Skip to content

Autoware車両制御トピック

【車両制御トピック】

Autowareの車両制御命令は[/twist_cmd]トピックで配信されます。
また、車両の現在速度が[/current_velocity]トピックで配信されます。
twist_cmd.twist.linear.xが目標速度(m/s)です。
twist_cmd.twist.angular.zが目標角速度(rad/s)です。
current_velocity.twist.linear.xが現在速度(m/s)です。
current_velocity.twist.angular.zが現在角速度(rad/s)です。

トピック監視

次の関数を実行すると、ROSの値取得が開始します。
デフォルトは10Hzで動作しています。
lib/autowarecar.py

1
2
3
4
5
    def listener(self):
        rospy.init_node('RoboCarROS', anonymous=True)
        rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd)
        rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity)
        rospy.spin()

init_node()にはrospyに登録するユニークな名前のノードを指定します。
rospyは一つのノードしか持つことが出来ません。
lib/autowarecar.py

1
        rospy.init_node('UNIQUE_NODE_NAME', anonymous=True)

次に、更新を監視するトピック名と、更新時に呼び出す関数を指定します。
lib/autowarecar.py

1
2
        rospy.Subscriber('/twist_cmd', TwistStamped, self.twist_cmd)
        rospy.Subscriber('/current_velocity', TwistStamped, self.current_velocity)

最後に、ノードが停止するまでpythonを終了させないようにします。
lib/autowarecar.py

1
        rospy.spin()

目標値

目標値が更新されると、self.wtist_cmdで指定した関数が呼び出されます。
目標速度の値はvalues.twist.linear.xになります。
Autoware1.9.1には後進は無いのですが、速度の値がマイナスになることがあります。
すべてプラスの値として使うようにします。
lib/autowarecar.py

1
2
3
    def twist_cmd(self, values):
        speed = abs(values.twist.linear.x)
        omega = values.twist.angular.z

目標角速度はvalues.twist.angular.zになります。
ラジコンのサーボ制御は0-180度で制御するように書いてあるので、ラジアンを0-180度に変換して反映します。
lib/autowarecar.py

1
        self.set_angle_rad(omega)

omegaとサーボの動作角は実際には一致しませんが、config.yamlのsteering_rateを変更して動きを調整します。
config.yaml

1
steering_rate: 0.5 # for TT-02 Drift

速度の値はここでは反映しません。実査の速度にしたいため、ここでは目標速度を保持するだけにします。

現在速度

現在速度が更新されると、self.current_velocityで指定した関数が呼び出されます。
現在速度と目標速度と比較して、速度を上限に変更します。
lib/autowarecar.py

1
2
3
4
5
6
    def current_velocity(self, values):
        current_velocity = values.twist.linear.x
        if current_velocity < self.target_speed:
            self.speed += 1
        elif current_velocity > self.target_speed:
            self.speed -= 1

最大速度は100、後進が無いため最低速度は0になります。
lib/autowarecar.py

1
2
3
4
        if self.speed >= 100:
            self.speed = 100
        elif self.speed <= 0:
            self.speed = 0

急発進を抑えるために、現在速度が遅い場合は、最大速度を25に制限します。
lib/autowarecar.py

1
2
        if self.speed > 25 and current_velocity < 0.1:
            self.speed = 25

速度を反映します。
lib/autowarecar.py

1
        self.set_speed(self.speed)