WSL2で起動するジョイスティックコントロール(python)
micro_ROS_joy2cmd_vel.py
""" micro_ROS_joy2cmd_vel.py 2025.4.20
【概要】トピック/joyから/cmd_velを生成する。
トピック/cmd_velをパブリッシュし、差動二輪ロボットを操作する。
トピック/encoderをサブスクライブし、/odomをパブリッシュする。
"""
import rclpy
from rclpy.node import Node
import sys
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from std_msgs.msg import String
from nav_msgs.msg import Odometry
from ros2_aruco_interfaces.msg import ArucoMarkers # Arマーカーのトピック
import time, datetime, math
from tf_transformations import quaternion_from_euler, euler_from_quaternion
import numpy as np
PPR_VAL = 11 # モーターの Pulses per Revolution
GEAR_RATIO = 168 # モーターの減速比
TIRE_DIAMETER = 0.07 # タイヤの直径(m)
TREAD = 0.2015 # タイヤの中心間の距離(m)
STOP_DISTANCE = 0.1 # 目的地手前で停止する距離
MIN_VEL_X = 0.3 # 直進速度下限 and 回転速度
MAX_VEL_X = 0.5 # 直進速度上限
class PublisherSubscriber(Node):
def __init__(self):
super().__init__('micro_ROS_joy2cmd_vel')
# サブスクライバー生成
self.sub = self.create_subscription(Joy, '/joy', self.joy_cb, 10) # ジョイスティックの出力
self.subscription = self.create_subscription(String, '/encoder', self.encoder_cb, 10) # エンコーダ値
self.sub_ar = self.create_subscription(ArucoMarkers, '/aruco_markers', self.ar_cb, 10) # ARマーカー
self.sub_waypoints = self.create_subscription(String, '/waypoints', self.waypoints_cb, 10) # ウェイポイント
# パブリッシャー生成
self.timer = self.create_timer(0.2, self.timer_callback) # /cmd_velを一定周期でパブリッシュする。
self.publisher = self.create_publisher(Twist, '/cmd_vel', 1)
self.pub = self.create_publisher(String, '/cmd_rob', 10)
self.cmd_vel = Twist()
self.pub_once = True # cmd_velが0ならば、1度だけパブリッシュする。
self.encoderL = 0 # /encoderから取得したエンコーダ値
self.encoderR = 0
self.encoderL_old = 0 # 一つ前の/encoderから取得したエンコーダ値
self.encoderR_old = 0
self.x = 0.0 # ロボットの座標
self.y = 0.0
self.theta = 0.0
self.notch = 0.1 # 加減速の大きさ
self.click_time = time.time() # ボタンの2度入力防止に使う
self.stop_flag = False # ストップボタンが押されたらTrue --- 確実に停止させる処理に入る。
self.stop_count = 15 # 実際に停止するまでスピード0の/cmd_velトピックを出し続ける回数
self.go_point = False # 指定点に移動中はTrue
self.init_turn = False # 指定点に向きを変えている最中はTrue
self.turn = False # 指定角度回転中はTrue
self.speed = MIN_VEL_X # 現在の直進速度
self.initial_distance = 0.0 # 移動開始時に計算した指定点までの移動距離。
self.start_x = 0.0 # 指定点に移動開始時の座標
self.start_y = 0.0
self.waypoints_ori = [[0, 1],[-1,1],[-1,0],[0.01,0.01]] # 巡回地点
self.waypoints_R1 = [[0.0,1.6],[-1.0,1.0],[-1.5,0.0],[-1.0,-0.5],[-0.7,0.5],[0.0,0.0]] # L1に割り付けられた巡回地点
self.waypoints_L1 = [[-0.7,0.5],[-1.0,-0.5],[-1.5,0.0],[-1.0,1.0],[0.0,1.6],[0.0,0.0]] # R1に割り付けられた巡回地点
self.waypoints_topic = [] # /waypointsトピックで指示されたウェイポイント
self.waypoints = [] # 巡回地点
self.go_waypoints = False # 巡回地点を巡回中
self.detect_mark = False # 移動先が原点(0,0)ならばTrue --- Trueの時だけマークを検出する。
self.start_time = 0.0 # 時間経過測定用
self.marker_id = 4 # ターゲットid
def timer_callback(self):
""" タイマーのコールバック関数
/cmd_velをパブリッシュする。
"""
if self.cmd_vel.linear.x == 0.0 and self.cmd_vel.angular.z == 0.0: # 並進速度が0の場合、1度だけパブリッシュする
if self.pub_once:
self.publisher.publish(self.cmd_vel)
self.pub_once = False
else:
#print(f"Timer angular.z: {self.cmd_vel.angular.z}")
self.publisher.publish(self.cmd_vel)
self.pub_once = True
def ar_cb(self, msg):
""" ARマーカーのサブスクライバ
"""
if self.detect_mark: # マークを検出する
for i, ar_id in enumerate(msg.marker_ids):
if ar_id != self.marker_id:
continue
ar_y = msg.poses[i].position.x * -1 # カメラから見たロボットの座標 変則的 !!!!!!!!!!!!!!
ar_x = msg.poses[i].position.y * -1 # カメラから見たロボットの座標 変則的 !!!!!!!!!!!!!!
ar_z = msg.poses[i].position.z
ar_q_x = msg.poses[i].orientation.x
ar_q_y = msg.poses[i].orientation.y
ar_q_z = msg.poses[i].orientation.z # マーカーまでの距離
ar_q_w = msg.poses[i].orientation.w
(roll, pitch, yaw) = euler_from_quaternion((ar_q_x, ar_q_y, ar_q_z, ar_q_w))
yaw = yaw * -1 # - math.pi/2 # カメラから見たロボットの角度 変則的 !!!!!!!!!!!!!!
yaw_d = np.rad2deg(yaw) # マーカーとカメラの角度
if not self.go_point: # 目的地に移動中はTrue
# X秒経過して完全に停止してからからオドメトリーを再設定する。
if self.start_time == 0.0:
self.start_time = time.time()
print('star timer set')
if time.time() - self.start_time > 1.5: # 1秒の経過を待つ。
print(f'===== オドメトリーをARマーカー{ar_id}で再設定した')
print(f' old x:{self.x:.2f}, y:{self.y:.2f}, yaw:{math.degrees(self.theta):.2f}°')
print(f' new x:{ar_x:.2f}, y:{ar_y:.2f}, yaw:{yaw_d:.1f}')
self.x = ar_x # カメラから見た位置情報で現在地を更新する。
self.y = ar_y
self.theta = yaw
self.detect_mark = False
self.start_time = 0.0
self.cmd_vel.linear.x = 0.0
self.cmd_vel.angular.z = 0.0
else:
self.cmd_vel.linear.x = 0.01 # 0ならばパブリッシュしないので小さな値を指定回数パブリッシュする。
self.cmd_vel.angular.z = 0.0
print('------- 停止の途中 ------')
def waypoints_cb(self, data):
""" /waypointのサブスクライバ
"""
res = data.data
ws = res.split(';') # ;分割で一つの命令を分離 --- カンマは使えない
for w in ws:
items = w.split(':') # :分割で指示と内容を分離
for item in items:
if item == "p":
posi = items[1].split('/')
px = float(posi[0])
py = float(posi[1])
self.waypoints_topic.append([px, py]) # ウェイポイントリストに追加する。
elif item == "c":
self.waypoints_topic.clear() # リストをクリアする。
print(f'トピックwaypoints:{res}')
print('waypointsの数: ',len(self.waypoints_topic))
def encoder_cb(self, data):
""" /encoderのサブスクライバ
"""
res = data.data
# サブスクライブしたデータからエンコーダ値を取り出し、
ws = res.split(',')
for w in ws:
items = w.split(':')
for item in items:
if item == "encL":
self.encoderL = int(items[1])
elif item == "encR":
self.encoderR = int(items[1])
Ld = self.encoderL - self.encoderL_old
Rd = self.encoderR - self.encoderR_old
#print(f'Encoder 左:{self.encoderL}, 差分:{Ld} 右:{self.encoderR}, 差分:{Rd}')
disL = Ld / (PPR_VAL * GEAR_RATIO) * math.pi * TIRE_DIAMETER
disR = Rd / (PPR_VAL * GEAR_RATIO) * math.pi * TIRE_DIAMETER
delta_d = (disL + disR) / 2.0
delta_theta = (disR - disL) / TREAD
self.x += delta_d * math.cos(self.theta)
self.y += delta_d * math.sin(self.theta)
self.theta += delta_theta
#self.theta = (self.theta + math.pi) % (2 * math.pi) - math.pi # 角度を-πからπの範囲に正規化
print(f"Odometry - x: {self.x:.2f}, y: {self.y:.2f}, theta: {math.degrees(self.theta):.2f}°")
self.encoderL_old = self.encoderL
self.encoderR_old = self.encoderR
def joy_cb(self, joy_msg):
""" /joyのサブスクライバ
"""
if (sum(joy_msg.axes) != 0 or sum(joy_msg.buttons) != 0): # 何かボタンが押された時だけ処理する。
if time.time() - self.click_time > 0.3: # 2度押し防止
self.click_time = time.time()
# 十字カーソル --- 直進、回転
if joy_msg.axes[7] == 1.0:
self.cmd_vel.linear.x += self.notch
elif joy_msg.axes[7] == -1.0:
self.cmd_vel.linear.x -= self.notch
elif joy_msg.axes[6] == 1.0:
self.cmd_vel.angular.z -= self.notch/2
elif joy_msg.axes[6] == -1.0:
self.cmd_vel.angular.z += self.notch/2
# 〇(オレンジ) --- 停止
elif joy_msg.buttons[1] == 1:
print('停止')
self.stop_flag = True
self.stop_count = 15
self.go_waypoints = False # 巡回
self.go_point = False # 目的地に移動
self.turn = False # 回転終了
self.initial_distance = 0.0
# △(緑) --- 原点に移動する
elif joy_msg.buttons[2] == 1:
# △ボタンを押すたびに、原点への移動と中止を繰り返す。
if self.detect_mark:
self.detect_mark = False
else:
self.target_x = 0.0
self.target_y = 0.0
print(f'原点に移動する')
self.go_point = True
self.init_turn = True
self.detect_mark = True # Trueならばマークを検出する。
# □(オレンジ) ------------------------------------- 処理内容再定義 !!!!!!
elif joy_msg.buttons[3] == 1:
# 指定点を巡回をTrueにする。
self.waypoints = self.waypoints_ori3.copy() # 巡回地点をワークにコピーする。
self.go_waypoints = True
print('巡回開始 R2')
# ×(青) --- odometryの値をクリアする。
elif joy_msg.buttons[0] == 1:
self.x = 0.0
self.y = 0.0
self.theta = 0.0
self.encoderL = 0
self.encoderR = 0
self.encoderL_old = 0
self.encoderR_old = 0
print("Odometry reset: x=0.0, y=0.0, theta=0.0°")
command = String()
command.data = 'C:0' # ESP32のエンコーダ値をクリアする。
self.pub.publish(command)
# L1 --- L1に割り付けられた巡回地点
elif joy_msg.buttons[4] == 1:
# 指定点を巡回をTrueにする。
self.waypoints = self.waypoints_L1.copy() # 巡回地点をワークにコピーする。
self.go_waypoints = True
print('巡回開始')
self.stop_flag = True
self.stop_count = 10
#self.tRad = math.pi * 4
#self.turn = True
#print('回転開始')
# R1 --- R1に割り付けられた巡回地点
elif joy_msg.buttons[5] == 1:
# 指定点を巡回をTrueにする。
self.waypoints = self.waypoints_R1.copy() # 巡回地点をワークにコピーする。
self.go_waypoints = True
print('巡回開始')
self.stop_flag = True
self.stop_count = 10
# OPTIONSボタン
elif joy_msg.buttons[9] == 1:
# 指定点を巡回をTrueにする。
self.waypoints = self.waypoints_topic.copy() # 巡回地点をワークにコピーする。
self.go_waypoints = True
print('巡回開始')
self.stop_flag = True
self.stop_count = 10
# 停止処理
elif self.stop_flag:
if self.stop_count > 0: # 実際に停止まで停止のcmd_velをパブリッシュする。
self.cmd_vel.linear.x = 0.01 # 0ならばパブリッシュしないので小さな値を指定回数パブリッシュする。
self.cmd_vel.angular.z = 0.0
self.stop_count -= 1
else:
self.cmd_vel.linear.x = 0.0 #
self.cmd_vel.angular.z = 0.0
self.stop_flag = False # 停止処理を完了する。
# 指定された地点に移動する
elif self.go_point:
# 初回時に移動距離を算出して保存
if self.initial_distance == 0.0:
self.start_x = self.x
self.start_y = self.y
self.initial_distance = math.sqrt((self.target_x - self.start_x)**2 + (self.target_y - self.start_y)**2)
# 目的地を向く。
if self.init_turn:
# ターゲット地点への角度（ラジアン）
angle_to_target = math.atan2(self.target_y - self.y, self.target_x - self.x)
# 角度の差を計算
angle_diff = angle_to_target - self.theta
# 角度の差を-πからπの範囲に正規化
angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi
#print(f" angle_diff: {math.degrees(angle_diff):.2f}°, angle_to_target: {math.degrees(angle_to_target):.2f}°, theta: {math.degrees(self.theta):.2f}°")
# 目的の角度に達するまで回転
if abs(angle_diff) > 0.2:
if angle_diff < 0:
self.cmd_vel.angular.z = MIN_VEL_X
else:
self.cmd_vel.angular.z = MIN_VEL_X * -1
else:
# 目的地を向いた。
self.cmd_vel.angular.z = 0.0
self.init_turn = False
print('--- 回転終了')
else:
# 目的地に進む。
distance_from_start = math.sqrt((self.x - self.start_x)**2 + (self.y - self.start_y)**2) # スタート地点からの距離
distance_to_target = math.sqrt((self.target_x - self.x)**2 + (self.target_y - self.y)**2) # 目的地までの距離
max_distance = max(self.initial_distance * 1.1, self.initial_distance + 0.1)
# スタート地点からの距離の距離が最初に計算した距離に近づいたら停止する。
if distance_from_start > self.initial_distance - 0.07:
print("■■■■■■■■■■■ 目的地に到着")
self.go_point = False
self.initial_distance = 0.0
#if len(self.waypoints) == 0 or (self.target_x == 0.0 and self.target_y == 0.0): # 最後の巡回地点の場合だけ完全に停止する。
if True: # 最後の巡回地点以外も必ず停止する。
self.stop_flag = True
self.stop_count = 10
# 目的地までの距離までの距離が一定以下なら停止する。
elif distance_to_target <= STOP_DISTANCE:
print("◇◇◇◇◇◇◇◇◇◇◇ 目的地に到着")
self.go_point = False
self.initial_distance = 0.0
#if len(self.waypoints) == 0 or (self.target_x == 0.0 and self.target_y == 0.0): # 最後の巡回地点の場合だけ完全に停止する。
if True: # 最後の巡回地点以外も必ず停止する。
self.stop_flag = True
self.stop_count = 10
else:
#print('進行中')
distance = math.sqrt((self.target_x - self.x)**2 + (self.target_y - self.y)**2)
# 目的地まで0.3m以上離れていたら加速する。
if distance > 0.3:
self.speed = min(self.speed + 0.01, MAX_VEL_X)
else:
self.speed = MIN_VEL_X
#--- 目的地までの角度が外れていたら向きを修正する。
# ターゲット地点への角度（ラジアン）
angle_to_target = math.atan2(self.target_y - self.y, self.target_x - self.x)
# 角度の差を計算
angle_diff = angle_to_target - self.theta
# 角度の差を-πからπの範囲に正規化
angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi
#print(f" angle_diff: {math.degrees(angle_diff):.2f}°, angle_to_target: {math.degrees(angle_to_target):.2f}°, theta: {math.degrees(self.theta):.2f}°")
if angle_diff > 0.08:
self.cmd_vel.angular.z = -0.05
elif angle_diff < -0.08:
self.cmd_vel.angular.z = 0.05
else:
self.cmd_vel.angular.z = 0.0
self.cmd_vel.linear.x = self.speed # 直進速度を設定する
# 指定点を巡回する。
elif self.go_waypoints:
if len(self.waypoints) == 0:
# してされた巡回地点をすべて巡回した巡回を終了する。
self.go_waypoints = False
print('巡回終了')
elif not self.go_point: # go_pointが終了するのを待つ。
if self.target_x == 0.0 and self.target_y == 0.0:
# 巡回先が原点ならば、マーカーでオドメトリーを再設定してから次の巡回に移る。
if not self.detect_mark:
p = self.waypoints.pop(0)
self.target_x = float(p[0])
self.target_y = float(p[1])
print(f'巡回点 x:{self.target_x}, y:{self.target_y}')
self.go_point = True
self.init_turn = True
if self.target_x == 0.0 and self.target_y == 0.0:
self.detect_mark = True # Trueならばマークを検出する。
else:
p = self.waypoints.pop(0)
self.target_x = float(p[0])
self.target_y = float(p[1])
print(f'巡回点 x:{self.target_x}, y:{self.target_y}')
self.go_point = True
self.init_turn = True
if self.target_x == 0.0 and self.target_y == 0.0:
self.detect_mark = True # Trueならばマークを検出する。
# 指定角度回転する。 --- 現在未使用
elif self.turn:
angle_diff = self.tRad - self.theta
print(f"回転角度: {angle_diff}")
if self.tRad > 0:
if angle_diff < 0.2:
self.cmd_vel.angular.z = 0.0
##self.publisher.publish(self.cmd_vel)
self.turn = False
print('--- 回転終了AW')
else:
self.cmd_vel.angular.z = -0.3
else:
if angle_diff > 0:
self.cmd_vel.angular.z = 0.0
self.turn = False
print('--- 回転終了W')
else:
self.cmd_vel.angular.z = 0.3
self.cmd_vel.linear.x = 0.0
#self.publisher.publish(self.cmd_vel)
def main():
rclpy.init(args=sys.argv)
pubsub = PublisherSubscriber()
rclpy.spin(pubsub)
pubsub.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
odometryをパブリッシュするコード(python)
micro_ROS_odometry.py
""" micro_ROS_odometry.py 2025.4.13
【概要】トピック/encoderをサブスクライブし、/odomをパブリッシュする。
【参照サイト】
ROS何も分からない人がロボット動かしてみた話
https://qiita.com/Kengokuma/items/f8b28b87b1e8c669905e
"""
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Quaternion
from tf_transformations import quaternion_from_euler
from std_msgs.msg import Int32MultiArray
from tf2_ros import TransformBroadcaster
from geometry_msgs.msg import TransformStamped
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from std_msgs.msg import String
import math
PPR_VAL = 11 # モーターの Pulses per Revolution
GEAR_RATIO = 168 # モーターの減速比
TIRE_DIAMETER = 0.07 # タイヤの直径(m)
TREAD = 0.2015 # タイヤの中心間の距離(m)
class OdometryNode(Node):
def __init__(self):
super().__init__('micro_ROS_odometry')
self.left_wheel_ticks = 0.0
self.right_wheel_ticks = 0.0
self.last_time = self.get_clock().now()
self.odom_publisher = self.create_publisher(Odometry, '/odom', 10)
self.subscription = self.create_subscription(String, '/encoder', self.encoder_cb, 10) # エンコーダ値
self.tf_broadcaster = TransformBroadcaster(self)
self.last_time = self.get_clock().now()
self.encoderL = 0 # /encoderから取得したエンコーダ値
self.encoderR = 0
self.encoderL_old = 0 # /odom作成用
self.encoderR_old = 0
self.x = 0.0 # ロボットの座標
self.y = 0.0
self.theta = 0.0
def encoder_cb(self, data):
""" /encoderのサブスクライバ
"""
res = data.data
# サブスクライブしたデータからエンコーダ値を取り出し、
ws = res.split(',')
for w in ws:
items = w.split(':')
for item in items:
if item == "encL":
self.encoderL = int(items[1])
elif item == "encR":
self.encoderR = int(items[1])
Ld = self.encoderL - self.encoderL_old
Rd = self.encoderR - self.encoderR_old
disL = Ld / (PPR_VAL * GEAR_RATIO) * math.pi * TIRE_DIAMETER
disR = Rd / (PPR_VAL * GEAR_RATIO) * math.pi * TIRE_DIAMETER
delta_d = (disL + disR) / 2.0
delta_theta = (disR - disL) / TREAD
self.x += delta_d * math.cos(self.theta)
self.y += delta_d * math.sin(self.theta)
self.theta += delta_theta
#self.theta = (self.theta + math.pi) % (2 * math.pi) - math.pi # 角度を-πからπの範囲に正規化
print(f"Odometry - x: {self.x:.2f}, y: {self.y:.2f}, theta: {math.degrees(self.theta):.2f}°")
self.encoderL_old = self.encoderL
self.encoderR_old = self.encoderR
current_time = self.get_clock().now()
delta_time = (current_time - self.last_time).nanoseconds / 1e9
self.last_time = current_time
#self.compute_odometry(delta_time)
# オドメトリメッセージを作成
odom = Odometry()
odom.header.stamp = self.get_clock().now().to_msg()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
# 座標と向き
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
# クォータニオン計算と設定
q = quaternion_from_euler(0, 0, self.theta)
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
self.odom_publisher.publish(odom)
# TF情報をブロードキャスト
transform = TransformStamped()
transform.header.stamp = self.get_clock().now().to_msg()
transform.header.frame_id = 'odom'
transform.child_frame_id = 'base_link'
transform.transform.translation.x = self.x
transform.transform.translation.y = self.y
transform.transform.translation.z = 0.0
transform.transform.rotation.x = q[0]
transform.transform.rotation.y = q[1]
transform.transform.rotation.z = q[2]
transform.transform.rotation.w = q[3]
self.tf_broadcaster.sendTransform(transform)
def main(args=None):
rclpy.init(args=args)
node = OdometryNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
ロボット搭載ESP32のコード(Arduino IDE 1.8.19)
micro_ROS_DCmotor_odometry_cmd_v2
#include <dummy.h>
/* micro_ROS_DCmotor_odometry_cmd_v2 2025.4.12
【概要】トピック/cmd_velを受信しモーターを駆動する。そのときのエンコーダの値を/encoderでパブリッシュする。
左右の車輪のencoder値からodometryをサブスクライブするノードで生成する。
twist型のトピックをsubscribeして、DC motorを回す。
受信したメッセージとモーター出力をOLEDに表示する。
トピック/cmd_robで動作を変更できる
subscribe: /recv (String)
publish: /echo (String)
*/
#include <Arduino.h>
#include <micro_ros_arduino.h>
#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/string.h>
#include <geometry_msgs/msg/twist.h>
// ESP32のpin定義
const int motorLPin1 = 32; // 左 ドライバーDRV8871のIN1へ
const int motorLPin2 = 33; // 左 ドライバーDRV8871のIN2へ
const int ENCO_L_A = 26; // 左 エンコーダー A のコードへ
const int ENCO_L_B = 25; // 左 エンコーダー B のコードへ
const int motorRPin1 = 12; // 左 ドライバーDRV8871のIN1へ
const int motorRPin2 = 13; // 左 ドライバーDRV8871のIN2へ
const int ENCO_R_A = 27; // 左 エンコーダー A のコードへ
const int ENCO_R_B = 14; // 左 エンコーダー B のコードへ 1
// LED, ブザーpin定義
const int LED_RED_pin = 18; // 赤のLEDへ
const int BLUE_RED_pin = 19; // 青のLEDへ
const int BUZZER_pin = 17; // ブザーへ
const int MAX_SPEED = 255; // 最大スピード
const int MIN_SPEED = 75; // 最小スピード
int LRdiff = 11; // 左右のモーター出力の補正値
const int INTERVAL = 500; // RPM計測の間隔(ms)
const int PPR_VAL = 11; // Pulses per Revolution
const int GEAR_RATIO = 168; // 減速比
#include <Wire.h> // SSD1306 --- OLED用
#include "SSD1306.h" // SSD1306用ライブラリを読み込み
SSD1306 display(0x3c, 21, 22); //SSD1306インスタンスの作成（I2Cアドレス,SDA,SCL）
//----- グローバル変数に行ごとの内容を保持
String line1 = ""; // 1行目
String line2 = ""; // 2行目
String line3 = ""; // 3行目
String line4 = ""; // 4行目
// 4行分の内容をOLEDに表示する関数
void update_display() {
display.clear(); // 画面をクリア
display.drawString(0, 0, line1); // 1行目の表示
display.drawString(0, 17, line2); // 2行目の表示
display.drawString(0, 34, line3); // 3行目の表示
display.drawString(0, 49, line4); // 4行目の表示
display.display(); // 画面を更新
}
volatile long L_encoderValue = 0; // エンコーダーのパルスカウンター
//volatile int L_encoderValue = 0; // エンコーダーのパルスカウンター
// 左エンコーダーのパルス発生割り込み処理
void IRAM_ATTR L_encoder_func() {
// A_val and B_val are HIGH / LOW
int A_state = digitalRead(ENCO_L_A);
int B_state = digitalRead(ENCO_L_B);
if (A_state == B_state) {
L_encoderValue--; // clockwise
} else {
L_encoderValue++; // counter clockwise
}
}
volatile long R_encoderValue = 0; // エンコーダーのパルスカウンター
//volatile int R_encoderValue = 0; // エンコーダーのパルスカウンター
// 右エンコーダーのパルス発生割り込み処理
void IRAM_ATTR R_encoder_func() {
// A_val and B_val are HIGH / LOW
int A_state = digitalRead(ENCO_R_A);
int B_state = digitalRead(ENCO_R_B);
if (A_state == B_state) {
R_encoderValue--; // clockwise
} else {
R_encoderValue++; // counter clockwise
}
}
// 文字列を区切り文字で分割する。
int split(String data, char delimiter, String *dst){
int index = 0;
int datalength = data.length();
for (int i = 0; i < datalength; i++) {
char tmp = data.charAt(i);
if ( tmp == delimiter ) {
index++;
}
else dst[index] += tmp;
}
return (index + 1);
}
rcl_node_t node;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_init_options_t init_options; //[追加]
rmw_init_options_t* rmw_options; //[追加]
// publisher
rcl_publisher_t publisher;
std_msgs__msg__String send_string_msg;
rclc_executor_t executor_pub;
rcl_timer_t timer;
// subscriber
rcl_subscription_t subscriber_st;
rcl_subscription_t subscriber_tw;
std_msgs__msg__String recv_string_msg;
geometry_msgs__msg__Twist msg;
#define STR_SIZE (100) //最大の受信文字数
rclc_executor_t executor_sub;
#define DOMAIN_ID 1 //[追加]
#define LED_PIN 2
#define RCCHECK(fn) \
{ \
rcl_ret_t temp_rc = fn; \
if ((temp_rc != RCL_RET_OK)) \
{ \
error_loop(); \
} \
}
#define RCSOFTCHECK(fn) \
{ \
rcl_ret_t temp_rc = fn; \
if ((temp_rc != RCL_RET_OK)) \
{ \
} \
}
int sub_cnt = 0; // subscription回数
/**
* @brief loop to indicate error with blinking LED
*
*/
void error_loop()
{
while (1)
{
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
//[追加] OLEDにエラーを表示する。
String err_msg = "";
if (LED_PIN == 0) {
err_msg = " Error";
} else {
err_msg = " ";
}
//------------------------------------------------------------------
display.drawString(0, 17, err_msg);
display.display(); // 画面を表示する
}
delay(200);
}
int cnt = 0;
void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
RCLC_UNUSED(last_call_time);
if (timer != NULL)
{
//RCSOFTCHECK(rcl_publish(&publisher, &send_string_msg, NULL));
//msg_heartbeat.data++;
cnt++;
}
}
/**
* @brief subscription callback executed at receiving a message
*
* @param msgin
*/
//------------------------------------------------------------------------ cmd_robのコールバック関数
void subscription_callback(const void *msgin)
{
//String str_cmd_rob = "";
const std_msgs__msg__String *recv_string_msg = (const std_msgs__msg__String *)msgin;
sub_cnt++; // subscription回数カウントアップ
//line1 = "msg_cnt: " + String(sub_cnt);
String str_cmd_rob = String(recv_string_msg->data.data);
line1 = str_cmd_rob;
String cmds[5] = {"\0"}; // ','で分割された文字列を格納する配列
String prms[3] = {"\0"}; // ':'で分割された文字列を格納する配列
char prm_char[4]; // コマンドを示す1文字
// 分割数 = 分割処理(文字列, 区切り文字, 配列)
int cmd_idx = split(str_cmd_rob, ',', cmds);
// ','で分割された文字列
for(int i = 0; i < cmd_idx; i++){
int prm_idx = split(cmds[i], ':', prms);
// ':'で分割された文字列
for(int j = 0; j < prm_idx; j++){
prms[j].toCharArray(prm_char, 2);
//----- パラメータ文字列の判定
if (prm_char[0] == 'D') {
LRdiff = prms[1].toInt();
} else if (prm_char[0] == 'C') {
L_encoderValue = 0;
R_encoderValue = 0;
}
line2 = prms[0];
line3 = prms[1];
}
}
update_display(); // OLEDに表示する。
}
// PWM補正計算関数(OpenAI作) 右の出力を下げる
int adjustLRdiff(int pwm) {
if (abs(pwm) >= 255) return LRdiff; // 最大値ならそのまま
if (abs(pwm) <= 75) return 0; // 最小値なら補正なし
return LRdiff * (abs(pwm) - 75) / (255 - 75); // 線形補間
}
//------------------------------------------------------------------------ cmd_velのコールバック関数
void subscription_callback_twist(const void *msgin) {
const geometry_msgs__msg__Twist *msg = (const geometry_msgs__msg__Twist *)msgin;
// cmd_velをPWM出力に変換する。
int motor_L = msg->linear.x * 255 + msg->angular.z * 255;
int motor_R = msg->linear.x * 255 + msg->angular.z * -255;
if (motor_R > 0)
motor_R -= adjustLRdiff(motor_R); // 右モーターのPWM値 補正値反映
else
motor_R += adjustLRdiff(motor_R); // 右モーターのPWM値 補正値反映
//int motor_R = msg->linear.x * 255 + msg->angular.z * -255 - adjustLRdiff(motor_R); // 右モーターのPWM値 補正値反映
// PWM出力の上限値チェック(左)
if (abs(motor_L) > MAX_SPEED)
{
if (motor_L > 0)
motor_L = MAX_SPEED;
else
motor_L = -MAX_SPEED;
}
// PWM出力の下限値チェック(左)
else if (abs(motor_L) < MIN_SPEED)
motor_L = 0;
// PWM出力の上限値チェック(右)
if (abs(motor_R) > MAX_SPEED)
{
if (motor_R > 0)
motor_R = MAX_SPEED;
else
motor_R = -MAX_SPEED;
}
// PW<出力の下限値チェック(右)
else if (abs(motor_R) < MIN_SPEED)
motor_R = 0;
// モーター出力をドライバーへの出力に変換する(左)
int motor_L_IN1, motor_L_IN2;
if (motor_L > 0)
{
motor_L_IN1 = motor_L;
motor_L_IN2 = 0;
}
else
{
motor_L_IN1 = 0;
motor_L_IN2 = abs(motor_L);
}
// モーター出力をドライバーへの出力に変換する(右)
int motor_R_IN1, motor_R_IN2;
if (motor_R > 0)
{
motor_R_IN1 = motor_R;
motor_R_IN2 = 0;
}
else
{
motor_R_IN1 = 0;
motor_R_IN2 = abs(motor_R);
}
analogWrite(motorLPin1, motor_L_IN1);
analogWrite(motorLPin2, motor_L_IN2);
analogWrite(motorRPin1, motor_R_IN1);
analogWrite(motorRPin2, motor_R_IN2);
// 送信メッセージをpublish形式に変換する。
String s = "encL:" + String(L_encoderValue) + "," + "encR:" + String(R_encoderValue);
char strBuf[120];
s.toCharArray(strBuf, 120);
send_string_msg.data.size = s.length();
send_string_msg.data.data = strBuf;
//
RCSOFTCHECK(rcl_publish(&publisher, &send_string_msg, NULL));
//----- 1行目の内容を更新 ドライバーへの出力値(左)
char buf_l1[32];
sprintf(buf_l1, "L1: %03d, L2: %03d", motor_L_IN1, motor_L_IN2);
line1 = buf_l1;
//----- 2行目の内容を更新 エンコーダー
char buf_l2[32];
sprintf(buf_l2, "encd_L: %ld", L_encoderValue);
line2 = buf_l2;
//----- 3行目の内容を更新 ドライバーへの出力値(右)
char buf_l3[32];
sprintf(buf_l3, "R1: %03d, R2: %03d", motor_R_IN1, motor_R_IN2);
line3 = buf_l3;
//----- 4行目の内容を更新 エンコーダー
char buf_l4[32];
sprintf(buf_l4, "encd_R: %ld", R_encoderValue);
line4 = buf_l4;
update_display(); // OLEDに表示する。
}
void setup()
{
// 初期画面表示
display.init(); //ディスプレイを初期化
display.setFont(ArialMT_Plain_16); //フォントを設定
display.flipScreenVertically(); // 表示反転（ボードにLCDを固定する向きに応じて）
line1 = "micro_ROS"; // プログラム名表示
line2 = "_DCmotor";
line3 = "_odometry";
line4 = "_v2";
update_display(); // OLEDに表示する。 // 画面を更新
// DC motor
pinMode(motorLPin1 , OUTPUT);
pinMode(motorLPin2 , OUTPUT);
pinMode(motorRPin1 , OUTPUT);
pinMode(motorRPin2 , OUTPUT);
analogWrite(motorLPin1, 0);
analogWrite(motorLPin2, 0);
analogWrite(motorRPin1, 0);
analogWrite(motorRPin2, 0);
// エンコーダー設定(左)
pinMode(ENCO_L_A, INPUT_PULLUP);
pinMode(ENCO_L_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCO_L_B), L_encoder_func, RISING);
pinMode(ENCO_R_A, INPUT_PULLUP);
pinMode(ENCO_R_B, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(ENCO_R_B), R_encoder_func, RISING);
// LED, ブザーpin設定
pinMode(LED_RED_pin , OUTPUT);
pinMode(BLUE_RED_pin , OUTPUT);
pinMode(BUZZER_pin , OUTPUT);
digitalWrite(LED_RED_pin, LOW );
digitalWrite(BLUE_RED_pin, LOW );
digitalWrite(BUZZER_pin, LOW );
//set_microros_transports();
set_microros_wifi_transports("aterm-2b4139-a", "3e00cfa4ba409", "192.168.0.122", 8888); // 63:dell2, 121:Raspi5
delay(3000);
allocator = rcl_get_default_allocator();
// Init options to use domain id ---- DOMEIN IDを設定する
init_options = rcl_get_zero_initialized_init_options();
RCCHECK(rcl_init_options_init(&init_options, allocator));
rmw_options = rcl_init_options_get_rmw_init_options(&init_options);
rcl_init_options_set_domain_id(&init_options, (size_t)DOMAIN_ID);
RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "micro_ros_cmd_vel_encoder", "", &support));
// create subscriber --------------------------------------------------------------- cmd_rob
RCCHECK(rclc_subscription_init_default(
&subscriber_st,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
"cmd_rob"));
// create subscriber --------------------------------------------------------------- cmd_vel
RCCHECK(rclc_subscription_init_default(
&subscriber_tw,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
"cmd_vel"));
// create publisher --------------------------------------------------------------- encoder
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
"encoder"));
// create timer, called every 1000 ms to publish heartbeat
const unsigned int timer_timeout = 500;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// create executor ---- publisher
RCCHECK(rclc_executor_init(&executor_pub, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor_pub, &timer));
// create executor ---- subscriber
RCCHECK(rclc_executor_init(&executor_sub, &support.context, 2, &allocator));
RCCHECK(rclc_executor_add_subscription(&executor_sub, &subscriber_st, &recv_string_msg, &subscription_callback, ON_NEW_DATA));
RCCHECK(rclc_executor_add_subscription(&executor_sub, &subscriber_tw, &msg, &subscription_callback_twist, ON_NEW_DATA));
// char型変数を入れる配列を確保する -------- 絶対必要
recv_string_msg.data.data = (char * )malloc(STR_SIZE * sizeof(char));
recv_string_msg.data.size = 0;
recv_string_msg.data.capacity = STR_SIZE;
}
void loop()
{
delay(10);
RCCHECK(rclc_executor_spin_some(&executor_pub, RCL_MS_TO_NS(100)));
RCCHECK(rclc_executor_spin_some(&executor_sub, RCL_MS_TO_NS(100)));
}