ROS2 navigation stack odometryの出力
この記事の目的
ROS2のnavigation stack用にpython scriptからodometryを出力する方法をまとめる。 ROS2とnavigagiton2 は下記記事に従って導入済みであることを前提とする。 sd08419ttic.hatenablog.com
1. スクリプトの構成
ROS2のpythonスクリプトの記述の実装方法は下記のチュートリアルに記載されている。
ROSと異なり、rclpyのNodeを継承して処理を記述し、一定周期に呼び出す処理をcallback関数で記述するように変更されている。 簡潔に記述するため、ROS2のノード処理(ノード立ち上げ、周期処理、終了)およびトピック通信(publish/subscribe)と制御に関する処理(自己位置の計算など)を別ファイルに記載する。
1. publish/subscribe処理の実装
ROS2でodometryを10msecでpublishし続けるスクリプトは下記のように記述できる。 下記スクリプトはOdomSimulatorNode生成時のinit関数でodometryのpublisher初期化、cmd_velのsubscribe設定、周期処理呼び出しのtimer設定をしている。 その後、ノード実行中はcmd_velのsubscribe成功時にsub_cmdvel_callbackで受信内容の保存、20msec周期でtimer_callbackから位置情報の更新とpublish、TFのbroadcastを行う。位置情報・TFの計算はodom_simulator_func.pyに記述する。
#!/usr/bin/env python # -*- coding: utf-8 -*- import pandas as pd # import for ros function import rclpy from rclpy.node import Node import tf2_py from geometry_msgs.msg import PoseStamped, Twist from std_msgs.msg import Header from nav_msgs.msg import Path, Odometry from odom_simulator_func import OdomSimlatorFunc from tf2_ros.transform_broadcaster import TransformBroadcaster class OdomSimulatorNode(Node): ################## # Initialization # ################## def __init__(self): super().__init__('Odom_Simulator') #initialize publisher self.OdometryPub_ = self.create_publisher(Odometry, '/odom', 10) self._tf_Odompublisher = TransformBroadcaster(self) #initialize subscriber self.CmdvelSub = self.create_subscription( Twist, '/cmd_vel', self.sub_cmdvel_callback, 10) self.CmdvelSub # prevent unused variable warning #timer configuration timer_period = 0.02 # seconds self.timer = self.create_timer(timer_period, self.timer_callback) #self.path_follow_func = Path_follow_func() self.cnt = 0 self.odomSimFunc = OdomSimlatorFunc() def timer_callback(self): # publish odom data #cmdvel = self.pathfollowerFunc.update_cmd_vel() #odom = Odometry() odom, tf_odom = self.odomSimFunc.update_odom() #calc cmdvel odom.header.stamp = self.get_clock().now().to_msg() self.OdometryPub_.publish(odom) tf_odom.header.stamp = self.get_clock().now().to_msg() self._tf_Odompublisher.sendTransform(tf_odom) self.cnt += 1 if self.cnt %10 ==0: self.get_logger().info('Publishing Count: "%s"' % self.cnt) #subscriber call back for Odometry def sub_cmdvel_callback(self, msg): self.odomSimFunc.set_cmdvel_from_subscriber(msg) test = Twist() self.get_logger().info('I heard: "%s"' % msg.linear) def main(args=None): rclpy.init(args=args) odomSimNode = OdomSimulatorNode() rclpy.spin(odomSimNode) # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) odomSimNode.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()
2. 位置情報・TFの計算方法
位置情報・TFの計算処理は下記のように記述できる。cmd_velで受信した目標速度・ヨーレートから座標を更新する。 ROS2ではTFからクォータニオンとオイラー角を変換する関数がなくなってしまったのでtf_lib ファイルに変換処理を記述する。
#!/usr/bin/env python # -*- coding: utf-8 -*- import math # import for ros function from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3 from std_msgs.msg import Header from nav_msgs.msg import Odometry from geometry_msgs.msg import TransformStamped from tf_lib import euler_from_quaternion, quaternion_from_euler class OdomSimlatorFunc(): ################## # Initialization # ################## def __init__(self): #Save path_plan flag self.save_path_as_csv = True #Cmd_vel receive flag (auto control) self.subscribe_cmd_vel = False #Parameter self.MAX_SPEED_KMH = 30.0 # MAX vehicle speed [km/h] self.MIN_SPEED_KMH = -20.0 # MINIMUM vehicle speed [km/h] self.MAX_SPEED_MPS = self.MAX_SPEED_KMH / 3.6 # MAX vehicle speed [km/h] self.MIN_SPEED_MPS = self.MIN_SPEED_KMH / 3.6 # MAX vehicle speed [km/h] self.MAX_YAW_RATE = 1.57 # MAX YAWRATE [rad/s] #initiali position initPosx = 0.0 initPosy = 0.0 #Initialize odometry header self.odom_header = Header() self.odom_header.frame_id = "map" # Initialize pose info self.sim_pose = Pose() self.sim_pose.position.x = initPosx self.sim_pose.position.y = initPosy self.sim_pose.position.z = 0.0 self.sim_pose.orientation.x = 0.0 self.sim_pose.orientation.y = 0.0 self.sim_pose.orientation.z = 0.0 # initialize twist info self.sim_twist = Twist() # Initialize odometry info self.sim_odom = Odometry() self.sim_odom.header = self.odom_header self.sim_odom.child_frame_id = "base_link" self.sim_odom.pose.pose = self.sim_pose self.sim_odom.twist.twist = self.sim_twist self.cmdvel_linear_x = 0.0 self.cmdvel_linear_y = 0.0 self.cmdvel_angular_z = 0.0 ############################################# # Update odometry form User request cmd_vel # ############################################# def update_odom(self): #Update Vehicle Pose sampletime = 0.01 #calculate by 10msec e = euler_from_quaternion(self.sim_pose.orientation.x, self.sim_pose.orientation.y, self.sim_pose.orientation.z, self.sim_pose.orientation.w) yaw_euler = e[2] #update pose from user request self.sim_pose.position.x = self.sim_pose.position.x + self.cmdvel_linear_x*sampletime*math.cos(yaw_euler) self.sim_pose.position.y = self.sim_pose.position.y + self.cmdvel_linear_x*sampletime*math.sin(yaw_euler) updated_yaw = e[2] + self.cmdvel_angular_z*sampletime updated_quaternion = quaternion_from_euler(0, 0, updated_yaw) self.sim_pose.orientation.x = updated_quaternion[0] self.sim_pose.orientation.y = updated_quaternion[1] self.sim_pose.orientation.z = updated_quaternion[2] self.sim_pose.orientation.w = updated_quaternion[3] #update timestamp #self.odom_header.stamp = rospy.Time.now() self.sim_odom.header = self.odom_header self.sim_odom.pose.pose = self.sim_pose self.sim_odom.twist.twist = self.sim_twist #update TF map_frame = TransformStamped() map_frame.header.frame_id = 'map' map_frame.child_frame_id = 'odom' map_frame.transform.translation.x = self.sim_pose.position.x map_frame.transform.translation.y = self.sim_pose.position.y map_frame.transform.translation.z = 0.0 map_frame.transform.rotation.w = self.sim_pose.orientation.w map_frame.transform.rotation.x = self.sim_pose.orientation.x map_frame.transform.rotation.y = self.sim_pose.orientation.y map_frame.transform.rotation.z = self.sim_pose.orientation.z return self.sim_odom, map_frame ##################### # cmdvel subscriber # ##################### def set_cmdvel_from_subscriber(self, msg): self.cmdvel_linear_x =msg.linear.x self.cmdvel_angular_z =msg.angular.z
tf_lib.py (座標変換処理)
#!/usr/bin/env python # -*- coding: utf-8 -*- import numpy as np import math def euler_from_quaternion(x,y,z,w): """ Converts quaternion (w in last place) to euler roll, pitch, yaw quaternion = [x, y, z, w] Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. """ sinr_cosp = 2 * (w * x + y * z) cosr_cosp = 1 - 2 * (x * x + y * y) roll = np.arctan2(sinr_cosp, cosr_cosp) sinp = 2 * (w * y - z * x) pitch = np.arcsin(sinp) siny_cosp = 2 * (w * z + x * y) cosy_cosp = 1 - 2 * (y * y + z * z) yaw = np.arctan2(siny_cosp, cosy_cosp) return roll, pitch, yaw def quaternion_from_euler(roll, pitch, yaw): """ Converts euler roll, pitch, yaw to quaternion (w in last place) quat = [x, y, z, w] Bellow should be replaced when porting for ROS 2 Python tf_conversions is done. """ cy = math.cos(yaw * 0.5) sy = math.sin(yaw * 0.5) cp = math.cos(pitch * 0.5) sp = math.sin(pitch * 0.5) cr = math.cos(roll * 0.5) sr = math.sin(roll * 0.5) q = [0] * 4 q[0] = cy * cp * cr + sy * sp * sr q[1] = cy * cp * sr - sy * sp * cr q[2] = sy * cp * sr + cy * sp * cr q[3] = sy * cp * cr - cy * sp * sr return q
上記処理を実行し、cmd_velを出力するとodomの出力・TFの発行がされていることをrviz2などで確認できる。 cmd_velは下記コマンド等で発行可能
ros2 topic pub /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1}, angular: {z: 0.3}}'
ソースコードは下記にまとめている。
