feat(accel_brake_map_calibrator): add test scripts for acc/brake cmd …

…generator (#1814)

* feat(accel_brake_map_calibrator): add test scripts for acc/brake cmd generator

Signed-off-by: Takamasa Horibe <[email protected]>

* update readme

Signed-off-by: Takamasa Horibe <[email protected]>

* fix names

Signed-off-by: Takamasa Horibe <[email protected]>

Signed-off-by: Takamasa Horibe <[email protected]>

vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/CMakeLists.txt

@@ -13,6 +13,9 @@ ament_target_dependencies(accel_brake_map_calibrator)
 install(
   PROGRAMS
   scripts/__init__.py
+  scripts/actuation_cmd_publisher.py
+  scripts/accel_tester.py
+  scripts/brake_tester.py
   scripts/config.py
   scripts/delay_estimator.py
   scripts/plotter.py

vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/README.md

@@ -138,3 +138,23 @@ You can also save accel and brake map in the default directory where Autoware re
 | progress_file_output     | bool   | if true, it will output a log and csv file of the update process.                                                                                                                 | false                                                    |
 | default_map_dir          | str    | directory of default map                                                                                                                                                          | [directory of *raw_vehicle_cmd_converter*]/data/default/ |
 | calibrated_map_dir       | str    | directory of calibrated map                                                                                                                                                       | [directory of *accel_brake_map_calibrator*]/config/      |
+
+## Test utility scripts
+
+### Constant accel/brake command test
+
+These scripts are useful to test for accel brake map calibration. These generate an `ActuationCmd` with a constant accel/brake value given interactively by a user through CLI.
+
+- accel_tester.py
+- brake_tester.py
+- actuation_cmd_publisher.py
+
+The `accel/brake_tester.py` receives a target accel/brake command from CLI. It sends a target value to `actuation_cmd_publisher.py` which generates the `ActuationCmd`. You can run these scripts by the following commands in the different terminals, and it will be as in the screenshot below.
+
+```bash
+ros2 run accel_brake_map_calibrator accel_tester.py
+ros2 run accel_brake_map_calibrator brake_tester.py
+ros2 run accel_brake_map_calibrator actuation_cmd_publisher.py
+```
+
+![actuation_cmd_publisher_util](./media/actuation_cmd_publisher_util.png)

vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/accel_tester.py

@@ -0,0 +1,57 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# Copyright 2022 Tier IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rclpy
+from rclpy.node import Node
+from tier4_debug_msgs.msg import Float32Stamped
+
+MAX_ACCEL = 1.0  # [-]
+MIN_ACCEL = 0.0  # [-]
+
+
+class AccelTester(Node):
+    def __init__(self):
+        super().__init__("vehicle_accel_tester")
+        self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/accel", 1)
+
+    def run(self):
+        while rclpy.ok():
+            value = float(input(f"target accel [{MIN_ACCEL} ~ {MAX_ACCEL} -] > "))
+            if value > MAX_ACCEL:
+                print("input value is larger than max accel!" + f"input: {value} max: {MAX_ACCEL}")
+                value = MAX_ACCEL
+            elif value < MIN_ACCEL:
+                print("input value is smaller than min accel!" + f"input: {value} min: {MIN_ACCEL}")
+                value = MIN_ACCEL
+
+            msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value)
+
+            self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    accel_tester = AccelTester()
+    accel_tester.run()
+
+    accel_tester.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py

@@ -0,0 +1,83 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# Copyright 2022 Tier IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from autoware_auto_vehicle_msgs.msg import GearCommand
+import rclpy
+from rclpy.node import Node
+from tier4_debug_msgs.msg import Float32Stamped
+from tier4_vehicle_msgs.msg import ActuationCommandStamped
+
+
+class ActuationCmdPublisher(Node):
+    def __init__(self):
+        super().__init__("actuation_cmd_publisher")
+        self.target_brake = 0.0
+        self.target_accel = 0.0
+
+        self.sub_acc = self.create_subscription(
+            Float32Stamped, "/vehicle/tester/accel", self.on_accel, 1
+        )
+        self.sub_brk = self.create_subscription(
+            Float32Stamped, "/vehicle/tester/brake", self.on_brake, 1
+        )
+        self.pub_actuation_cmd = self.create_publisher(
+            ActuationCommandStamped, "/control/command/actuation_cmd", 1
+        )
+        self.pub_gear_cmd = self.create_publisher(GearCommand, "/control/command/gear_cmd", 1)
+        timer_period = 0.02  # seconds
+        self.timer = self.create_timer(timer_period, self.on_timer)
+
+    def on_brake(self, msg):
+        self.target_brake = msg.data
+        print(f"Set target brake : {self.target_brake}")
+
+    def on_accel(self, msg):
+        self.target_accel = msg.data
+        print(f"Set target accel : {self.target_accel}")
+
+    def on_timer(self):
+        msg_actuation_cmd = ActuationCommandStamped()
+        msg_actuation_cmd.actuation.steer_cmd = 0.0
+        msg_actuation_cmd.header.stamp = self.get_clock().now().to_msg()
+        msg_actuation_cmd.header.frame_id = "base_link"
+        msg_actuation_cmd.actuation.accel_cmd = self.target_accel
+        msg_actuation_cmd.actuation.brake_cmd = self.target_brake
+        self.pub_actuation_cmd.publish(msg_actuation_cmd)
+
+        msg_gear_cmd = GearCommand()
+        msg_gear_cmd.stamp = self.get_clock().now().to_msg()
+        msg_gear_cmd.command = GearCommand.DRIVE
+        self.pub_gear_cmd.publish(msg_gear_cmd)
+
+        print(
+            f"publish ActuationCommand with accel: {self.target_accel}, brake: {self.target_brake}"
+        )
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    actuation_cmd_publisher = ActuationCmdPublisher()
+
+    rclpy.spin(actuation_cmd_publisher)
+
+    actuation_cmd_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/brake_tester.py

@@ -0,0 +1,61 @@
+#! /usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+# Copyright 2022 Tier IV, Inc. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+import rclpy
+from rclpy.node import Node
+from tier4_debug_msgs.msg import Float32Stamped
+
+MAX_BRAKE = 1.0  # [-]
+MIN_BRAKE = 0.0  # [-]
+
+
+class BrakeTester(Node):
+    def __init__(self):
+        super().__init__("vehicle_brake_tester")
+        self.pub = self.create_publisher(Float32Stamped, "/vehicle/tester/brake", 1)
+
+    def run(self):
+        while rclpy.ok():
+            value = float(
+                input("target brake [" + str(MIN_BRAKE) + " ~ " + str(MAX_BRAKE) + "] > ")
+            )
+
+            if value > MAX_BRAKE:
+                print("input value is larger than max brake!" + f"input: {value} max: {MAX_BRAKE}")
+                value = MAX_BRAKE
+            elif value < MIN_BRAKE:
+                print("input value is smaller than min brake!" + f"input: {value} min: {MIN_BRAKE}")
+                value = MIN_BRAKE
+
+            msg = Float32Stamped(stamp=self.get_clock().now().to_msg(), data=value)
+
+            self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    brake_tester = BrakeTester()
+    brake_tester.run()
+
+    brake_tester.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()