feat: apply simulator update (#64)

* add launch file template

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* update control_mode_request

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* add awsim relay

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add awsim adapter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

---------

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

aichallenge/run_evaluation.bash

@@ -46,7 +46,7 @@ sleep 1
 
 # Start driving and wait for the simulation to finish
 echo "Waiting for the simulation"
-ros2 topic pub --once /control/control_mode_request_topic std_msgs/msg/Bool '{data: true}' >/dev/null
+ros2 service call /control/control_mode_request autoware_auto_vehicle_msgs/srv/ControlModeCommand '{mode: 1}' >/dev/null
 wait $PID_AWSIM
 
 # Stop recording rviz2

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/aichallenge_submit.launch.xml

@@ -1,11 +1,15 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
-  <log message="This is aichallenge_submit_launch."/>
-  <arg name="use_sim_time" default="true"/>
-  <arg name="launch_vehicle_interface" default="false"/>
-  <arg name="sensor_model" default="racing_kart_sensor_kit"/>
-  <arg name="simulation" default="true"/>
-  <log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>
+  <arg name="simulation"/>
+  <arg name="use_sim_time"/>
+  <arg name="sensor_model"/>
+  <arg name="launch_vehicle_interface"/>
+
+  <log message="The arguments for aichallenge_submit_launch."/>
+  <log message=" - simulation: $(var simulation)"/>
+  <log message=" - use_sim_time: $(var use_sim_time)"/>
+  <log message=" - sensor_model: $(var sensor_model)"/>
+  <log message=" - launch_vehicle_interface: $(var launch_vehicle_interface)"/>
 
   <include file="$(find-pkg-share aichallenge_submit_launch)/launch/reference.launch.xml" >
     <arg name="vehicle_model" value="racing_kart"/>

aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/urdf/sensor_kit.xacro

@@ -19,10 +19,10 @@
     </link>
 
     <!-- sensor -->
-    <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensor_kit_calibration.yaml')}"/>   
+    <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensor_kit_calibration.yaml')}"/>
     <!-- over write calibration file for simulation -->
     <xacro:if value="$(arg simulation)">
-      <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/awsim_sensor_kit_calibration.yaml')}"/>    
+      <xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/awsim_sensor_kit_calibration.yaml')}"/>
     </xacro:if>
     <!-- gnss -->
     <xacro:imu_macro

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.14)
+project(aichallenge_awsim_adapter)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/actuation_cmd_converter.cpp
+)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "ActuationCmdConverter"
+  EXECUTABLE actuation_cmd_converter
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="csv_path_accel_map" default="$(find-pkg-share aichallenge_submit_launch)/data/accel_map.csv"/>
+  <arg name="csv_path_brake_map" default="$(find-pkg-share aichallenge_submit_launch)/data/brake_map.csv"/>
+
+  <node pkg="aichallenge_awsim_adapter" exec="actuation_cmd_converter">
+    <param name="csv_path_accel_map" value="$(var csv_path_accel_map)"/>
+    <param name="csv_path_brake_map" value="$(var csv_path_accel_map)"/>
+  </node>
+</launch>

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>aichallenge_awsim_adapter</name>
+  <version>0.1.0</version>
+  <description>The aichallenge_awsim_adapter package</description>
+  <maintainer email="isamu.takagi@tier4.jp">Takagi Isamu</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>tier4_vehicle_msgs</depend>
+  <depend>raw_vehicle_cmd_converter</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.cpp

@@ -0,0 +1,92 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// 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.
+
+#include "actuation_cmd_converter.hpp"
+
+ActuationCmdConverter::ActuationCmdConverter(const rclcpp::NodeOptions & node_options)
+: Node("actuation_cmd_converter", node_options)
+{
+  using std::placeholders::_1;
+
+  // Parameters
+  const std::string csv_path_accel_map = declare_parameter<std::string>("csv_path_accel_map");
+  const std::string csv_path_brake_map = declare_parameter<std::string>("csv_path_brake_map");
+
+  // Subscriptions
+  sub_actuation_ = create_subscription<ActuationCommandStamped>(
+    "/control/command/actuation_cmd", 1, std::bind(&ActuationCmdConverter::on_actuation_cmd, this, _1));
+  sub_gear_ = create_subscription<GearReport>(
+    "/vehicle/status/gear_status", 1, std::bind(&ActuationCmdConverter::on_gear_report, this, _1));
+  sub_velocity_ = create_subscription<VelocityReport>(
+    "/vehicle/status/velocity_status", 1, std::bind(&ActuationCmdConverter::on_velocity_report, this, _1));
+
+  // Publishers
+  pub_ackermann_ = create_publisher<AckermannControlCommand>("/awsim/control_cmd", 1);
+
+  // Load accel/brake map
+  if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map)) {
+    RCLCPP_ERROR(get_logger(), "Cannot read accelmap. csv path = %s. stop calculation.", csv_path_accel_map.c_str());
+    throw std::runtime_error("Cannot read accelmap.");
+  }
+  if (!brake_map_.readBrakeMapFromCSV(csv_path_brake_map)) {
+    RCLCPP_ERROR(get_logger(), "Cannot read brakemap. csv path = %s. stop calculation.", csv_path_brake_map.c_str());
+    throw std::runtime_error("Cannot read brakemap.");
+  }
+}
+
+void ActuationCmdConverter::on_gear_report(const GearReport::ConstSharedPtr msg)
+{
+  gear_report_ = msg;
+}
+
+void ActuationCmdConverter::on_velocity_report(const VelocityReport::ConstSharedPtr msg)
+{
+  velocity_report_ = msg;
+}
+
+void ActuationCmdConverter::on_actuation_cmd(const ActuationCommandStamped::ConstSharedPtr msg)
+{
+  // Wait for input data
+  if (!gear_report_ || !velocity_report_) {
+    return;
+  }
+
+  const double velocity = std::abs(velocity_report_->longitudinal_velocity);
+  const double acceleration = get_acceleration(*msg, velocity);
+
+  // Publish ControlCommand
+  constexpr float nan = std::numeric_limits<double>::quiet_NaN();
+  AckermannControlCommand output;
+  output.stamp = msg->header.stamp;
+  output.lateral.steering_tire_angle = static_cast<float>(msg->actuation.steer_cmd);
+  output.lateral.steering_tire_rotation_rate = nan;
+  output.longitudinal.speed = nan;
+  output.longitudinal.acceleration = static_cast<float>(acceleration);
+  pub_ackermann_->publish(output);
+}
+
+double ActuationCmdConverter::get_acceleration(const ActuationCommandStamped & cmd, const double velocity)
+{
+  const double desired_pedal = cmd.actuation.accel_cmd - cmd.actuation.brake_cmd;
+  double ref_acceleration = 0.0;
+  if (desired_pedal > 0.0) {
+    accel_map_.getAcceleration(+desired_pedal, velocity, ref_acceleration);
+  } else {
+    brake_map_.getAcceleration(-desired_pedal, velocity, ref_acceleration);
+  }
+  return ref_acceleration;
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(ActuationCmdConverter)

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/actuation_cmd_converter.hpp

@@ -0,0 +1,55 @@
+// Copyright 2020 Tier IV, Inc.
+//
+// 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.
+
+#ifndef AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_
+#define AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_
+
+#include <raw_vehicle_cmd_converter/accel_map.hpp>
+#include <raw_vehicle_cmd_converter/brake_map.hpp>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/gear_report.hpp>
+#include <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
+#include <tier4_vehicle_msgs/msg/actuation_command_stamped.hpp>
+
+using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_vehicle_msgs::msg::GearReport;
+using autoware_auto_vehicle_msgs::msg::VelocityReport;
+using tier4_vehicle_msgs::msg::ActuationCommandStamped;
+
+class ActuationCmdConverter : public rclcpp::Node
+{
+public:
+  explicit ActuationCmdConverter(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Subscription<ActuationCommandStamped>::SharedPtr sub_actuation_;
+  rclcpp::Subscription<GearReport>::SharedPtr sub_gear_;
+  rclcpp::Subscription<VelocityReport>::SharedPtr sub_velocity_;
+  rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_ackermann_;
+
+  void on_actuation_cmd(const ActuationCommandStamped::ConstSharedPtr msg);
+  void on_gear_report(const GearReport::ConstSharedPtr msg);
+  void on_velocity_report(const VelocityReport::ConstSharedPtr msg);
+
+  double get_acceleration(const ActuationCommandStamped & cmd, const double velocity);
+  raw_vehicle_cmd_converter::AccelMap accel_map_;
+  raw_vehicle_cmd_converter::BrakeMap brake_map_;
+  GearReport::ConstSharedPtr gear_report_;
+  VelocityReport::ConstSharedPtr velocity_report_;
+};
+
+#endif  // AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_

aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml

@@ -1,35 +1,45 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
     <arg name="rviz_config" default="$(find-pkg-share aichallenge_system_launch)/config/debug_sensing.rviz" description="rviz config"/>
-    <log message="This is aichallenge_system_launch."/>
     <arg name="simulation" default="true"/>
     <arg name="use_sim_time" default="$(var simulation)"/>
     <arg name="run_rviz" default="$(var simulation)"/>
-    <arg name="launch_vehicle_interface" default="true"/>
     <arg name="sensor_model" default="racing_kart_sensor_kit"/>
-    <log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>
-    <log message="echo launch param simulation: $(var simulation)"/>
+    <arg name="launch_vehicle_interface" default="true"/>
 
+    <log message="The arguments for aichallenge_system_launch."/>
+    <log message=" - simulation: $(var simulation)"/>
+    <log message=" - use_sim_time: $(var use_sim_time)"/>
+    <log message=" - sensor_model: $(var sensor_model)"/>
+    <log message=" - launch_vehicle_interface: $(var launch_vehicle_interface)"/>
+
+    <!-- Main module of AI challenge 2024. -->
     <include file="$(find-pkg-share aichallenge_submit_launch)/launch/aichallenge_submit.launch.xml" >
-        <arg name="sensor_model" value="$(var sensor_model)"/>
+        <arg name="simulation" value="$(var simulation)"/>
         <arg name="use_sim_time" value="$(var use_sim_time)"/>
+        <arg name="sensor_model" value="$(var sensor_model)"/>
         <arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
-        <arg name="simulation" value="$(var simulation)"/>
     </include>
 
-    <!-- Workaround because the simulator cannot use the service. -->
-    <group if="$(var simulation)">
-        <node pkg="aichallenge_system_launch" exec="control_mode_adapter.py" output="screen"/>
+    <!-- Visualization. -->
+    <group if="$(var run_rviz)">
+        <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_config) -s $(find-pkg-share aichallenge_system_launch)/rviz/image/autoware.png"/>
         <node pkg="aichallenge_system_launch" exec="object_marker.py" output="screen"/>
     </group>
 
+    <!-- Initial Pose -->
     <node pkg="topic_tools" exec="relay" name="initial_pose_relay">
       <param name="input_topic" value="/initialpose"/>
       <param name="output_topic" value="/localization/initial_pose3d"/>
     </node>
 
-    <!-- RViz -->
-   <group if="$(var run_rviz)">
-        <node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_config) -s $(find-pkg-share aichallenge_system_launch)/rviz/image/autoware.png"/>
+    <!-- Adapters for simulation. -->
+    <group if="$(var simulation)">
+        <include file="$(find-pkg-share aichallenge_system_launch)/launch/mode/awsim.launch.xml"/>
+    </group>
+
+    <!-- Adapters for real vehicle. -->
+    <group unless="$(var simulation)">
+        <include file="$(find-pkg-share aichallenge_system_launch)/launch/mode/real.launch.xml"/>
     </group>
 </launch>

aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml

@@ -0,0 +1,35 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+    <!-- Workaround because the simulator cannot use the service. -->
+    <node pkg="aichallenge_system_launch" exec="control_mode_adapter.py" output="screen"/>
+
+    <!-- Adaptors for AWSIM -->
+    <group>
+        <include file="$(find-pkg-share aichallenge_awsim_adapter)/launch/aichallenge_awsim_adapter.launch.xml"/>
+    </group>
+
+    <node pkg="topic_tools" exec="relay" name="relay_gnss_pose">
+        <param name="input_topic" value="/awsim/gnss/pose"/>
+        <param name="output_topic" value="/sensing/gnss/pose"/>
+    </node>
+
+    <node pkg="topic_tools" exec="relay" name="relay_gnss_pose_cov">
+        <param name="input_topic" value="/awsim/gnss/pose_with_covariance"/>
+        <param name="output_topic" value="/sensing/gnss/pose_with_covariance"/>
+    </node>
+
+    <node pkg="topic_tools" exec="relay" name="relay_imu">
+        <param name="input_topic" value="/awsim/imu"/>
+        <param name="output_topic" value="/sensing/imu/imu_raw"/>
+    </node>
+
+    <node pkg="topic_tools" exec="relay" name="relay_steering">
+        <param name="input_topic" value="/awsim/steering_status"/>
+        <param name="output_topic" value="/vehicle/status/steering_status"/>
+    </node>
+
+    <node pkg="topic_tools" exec="relay" name="relay_velocity">
+        <param name="input_topic" value="/awsim/velocity_status"/>
+        <param name="output_topic" value="/vehicle/status/velocity_status"/>
+    </node>
+</launch>

aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/real.launch.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+</launch>

aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/script/control_mode_adapter.py

@@ -9,7 +9,7 @@ class ControlModeAdapterNode(rclpy.node.Node):
     def __init__(self):
         super().__init__("control_mode_adapter")
         self.sub = self.create_service(ControlModeCommand, "/control/control_mode_request", self.callback)
-        self.pub = self.create_publisher(Bool, "/control/control_mode_request_topic", 1)
+        self.pub = self.create_publisher(Bool, "/awsim/control_mode_request_topic", 1)
 
     def callback(self, req, res):
         msg = Bool()
@@ -32,4 +32,7 @@ def main(args=None):
     rclpy.shutdown()
 
 if __name__ == "__main__":
-    main()
+    try:
+        main()
+    except KeyboardInterrupt:
+        pass