Merge pull request #13 from booars/sync-upstream

chore: sync upstream
booars · Sep 4, 2024 · 130d37a · 130d37a
2 parents d559207 + 08af797
commit 130d37a
Show file tree

Hide file tree

Showing 11 changed files with 111 additions and 4 deletions.
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/CMakeLists.txt
@@ -3,4 +3,4 @@ project(aichallenge_submit_launch)
 
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
-ament_auto_package(INSTALL_TO_SHARE launch map config)
+ament_auto_package(INSTALL_TO_SHARE launch map data config)
diff --git a/...ge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/converter.param.yaml b/...ge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/converter.param.yaml
@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    use_steer_ff: true
+    use_steer_fb: true
+    is_debugging: false
+    steer_pid:
+      kp: 150.0
+      ki: 15.0
+      kd: 0.0
+      max: 8.0
+      min: -8.0
+      max_p: 8.0
+      min_p: -8.0
+      max_i: 8.0
+      min_i: -8.0
+      max_d: 0.0
+      min_d: 0.0
+      invalid_integration_decay: 0.97
diff --git a/...orkspace/src/aichallenge_submit/aichallenge_submit_launch/config/imu_corrector.param.yaml b/...orkspace/src/aichallenge_submit/aichallenge_submit_launch/config/imu_corrector.param.yaml
@@ -0,0 +1,8 @@
+/**:
+  ros__parameters:
+    angular_velocity_offset_x: 0.0    # [rad/s]
+    angular_velocity_offset_y: 0.0    # [rad/s]
+    angular_velocity_offset_z: 0.0    # [rad/s]
+    angular_velocity_stddev_xx: 0.03  # [rad/s]
+    angular_velocity_stddev_yy: 0.03  # [rad/s]
+    angular_velocity_stddev_zz: 0.03  # [rad/s]
diff --git a/...ce/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/...ce/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml
@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    lanelet2_map_projector_type: MGRS  # Options: MGRS, UTM
+    center_line_resolution: 5.0        # [m]
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/accel_map.csv
@@ -0,0 +1,7 @@
+default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
+0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
+0.1,0.6,0.42,0.24,0.18,0.12,0.05,-0.08,-0.16,-0.2,-0.24,-0.28
+0.2,1.15,0.98,0.78,0.6,0.48,0.34,0.26,0.2,0.1,0.05,-0.03
+0.3,1.75,1.6,1.42,1.3,1.14,1,0.9,0.8,0.72,0.64,0.58
+0.4,2.65,2.48,2.3,2.13,1.95,1.75,1.58,1.45,1.32,1.2,1.1
+0.5,3.3,3.25,3.12,2.92,2.68,2.35,2.17,1.98,1.88,1.73,1.61
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/brake_map.csv b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/brake_map.csv
@@ -0,0 +1,10 @@
+default,0.0, 1.39, 2.78, 4.17, 5.56, 6.94, 8.33, 9.72, 11.11, 12.50, 13.89
+0,0.3,-0.05,-0.3,-0.39,-0.4,-0.41,-0.42,-0.44,-0.46,-0.48,-0.5
+0.1,0.29,-0.06,-0.31,-0.4,-0.41,-0.42,-0.43,-0.45,-0.47,-0.49,-0.51
+0.2,-0.38,-0.4,-0.72,-0.8,-0.82,-0.85,-0.87,-0.89,-0.91,-0.94,-0.96
+0.3,-1,-1.04,-1.48,-1.55,-1.57,-1.59,-1.61,-1.63,-1.631,-1.632,-1.633
+0.4,-1.48,-1.5,-1.85,-2.05,-2.1,-2.101,-2.102,-2.103,-2.104,-2.105,-2.106
+0.5,-1.49,-1.51,-1.86,-2.06,-2.11,-2.111,-2.112,-2.113,-2.114,-2.115,-2.116
+0.6,-1.5,-1.52,-1.87,-2.07,-2.12,-2.121,-2.122,-2.123,-2.124,-2.125,-2.126
+0.7,-1.51,-1.53,-1.88,-2.08,-2.13,-2.131,-2.132,-2.133,-2.134,-2.135,-2.136
+0.8,-2.18,-2.2,-2.7,-2.8,-2.9,-2.95,-2.951,-2.952,-2.953,-2.954,-2.955
diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/steer_map.csv b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/data/steer_map.csv
@@ -0,0 +1,14 @@
+default,-0.6,-0.5,-0.4,-0.3,-0.2,-0.1,0,0.1,0.2,0.3,0.4,0.5,0.6
+-12,-0.29,-0.3234236132,-0.3468991362,-0.37274847,-0.4288688461,-0.4696573,-0.4781014157,-0.49,-0.51,-0.52,-0.53,-0.54,-0.55
+-10,-0.2,-0.2451308686,-0.2667782734,-0.3090752469,-0.3575200568,-0.3874868999,-0.4041493831,-0.42,-0.43,-0.44,-0.45,-0.46,-0.47
+-8,-0.1,-0.1586739777,-0.2020698818,-0.2317654357,-0.2778998646,-0.3029839842,-0.3188172953,-0.32,-0.33,-0.34,-0.35,-0.36,-0.37
+-6,-0.0404761584,-0.0806574159,-0.1208386734,-0.1559621241,-0.1888907059,-0.210790018,-0.2322336736,-0.24,-0.25,-0.26,-0.27,-0.28,-0.29
+-4,0.01,-0.01428569928,-0.04,-0.06805018099,-0.09718925222,-0.1189509092,-0.1361293637,-0.14,-0.153044463,-0.1688321844,-0.18,-0.2,-0.22
+-2,0.06,0.05,0.03,0.01,-0.006091655083,-0.03546033903,-0.05319488695,-0.06097627388,-0.07576251508,-0.09165178816,-0.1,-0.12,-0.14
+0,0.11,0.09,0.07,0.05,0.03,0.01,-0.0004106386541,-0.01,-0.03,-0.05,-0.07,-0.09,-0.11
+2,0.15,0.13,0.11,0.09,0.07812978496,0.06023545297,0.04386326928,0.02314813566,0.003779338416,-0.01390526686,-0.0324840879,-0.05106290894,-0.06964172998
+4,0.24,0.22,0.2,0.1839795042,0.1649754952,0.1455879759,0.1274837062,0.1020632549,0.0861964856,0.06971503353,0.04541294018,0.01308869356,-0.01923555307
+6,0.33,0.31,0.29,0.274963497,0.2447295892,0.2337353319,0.2126138313,0.1767700907,0.1578757866,0.1440584148,0.1136331403,0.0827443595,0.05185557872
+8,0.43,0.41,0.39,0.3714915121,0.3299578073,0.3156403746,0.2997845963,0.2500495071,0.2339668568,0.2153133621,0.1815127859,0.156071251,0.1306297162
+10,0.5,0.49,0.48,0.4629455165,0.4210353203,0.3977027236,0.3865034288,0.3250957262,0.312237913,0.2885123051,0.2474220915,0.2123900615,0.1773580315
+12,0.56,0.54,0.53,0.5151036525,0.5046070554,0.4897214196,0.4687756354,0.4036994672,0.3812674227,0.3496883008,0.32482655,0.2783538423,0.2318811345
diff --git a/...ge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/...ge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -68,6 +68,7 @@
       <arg name="enable_yaw_bias_estimation" value="false"/>
       <arg name="tf_rate" value="50.0"/>
       <arg name="twist_smoothing_steps" value="1"/>
+      <arg name="pose_smoothing_steps" value="1"/>
       <arg name="input_initial_pose_name" value="/localization/initial_pose3d"/>
       <arg name="input_pose_with_cov_name" value="/localization/imu_gnss_poser/pose_with_covariance"/>
       <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
@@ -250,6 +251,26 @@
 
   </group> <!-- map -->
 
+  <!-- vehicle -->
+  <group if="$(var launch_vehicle_interface)">
+    <include file="$(find-pkg-share raw_vehicle_cmd_converter)/launch/raw_vehicle_converter.launch.xml">
+      <arg name="converter_param_path" value="$(find-pkg-share aichallenge_submit_launch)/config/converter.param.yaml"/>
+      <arg name="csv_path_accel_map" value="$(find-pkg-share aichallenge_submit_launch)/data/accel_map.csv"/>
+      <arg name="csv_path_brake_map" value="$(find-pkg-share aichallenge_submit_launch)/data/brake_map.csv"/>
+
+      <arg name="max_throttle" value="1.0"/>
+      <arg name="max_brake" value="1.0"/>
+      <arg name="convert_accel_cmd" value="true"/>
+      <arg name="convert_brake_cmd" value="true"/>
+      <arg name="convert_steer_cmd" value="false"/>
+
+      <arg name="input_control_cmd" value="/control/command/control_cmd"/>
+      <arg name="input_odometry" value="/localization/kinematic_state"/>
+      <arg name="input_steering" value="/vehicle/status/steering_status"/>
+      <arg name="output_actuation_cmd" value="/control/command/actuation_cmd"/>
+    </include>
+  </group>
+
   <!-- API -->
   <group>
     <!-- default_ad_api -->

diff --git a/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp b/aichallenge/workspace/src/aichallenge_submit/goal_pose_setter/src/goal_pose_setter_node.cpp
@@ -89,7 +89,7 @@ void GoalPosePublisher::on_timer()
         msg->pose.orientation.w = this->get_parameter("goal.orientation.w").as_double();
 
         goal_publisher_->publish(*msg);
-        RCLCPP_INFO(this->get_logger(), "Publishing goal pose");
+        RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000 /*ms*/, "Publishing goal pose");
     }
 }
 

diff --git a/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp b/aichallenge/workspace/src/aichallenge_submit/imu_corrector/src/imu_corrector_core.cpp
@@ -42,6 +42,17 @@ geometry_msgs::msg::Vector3 transformVector3(
   return vec_stamped_transformed.vector;
 }
 
+geometry_msgs::msg::Quaternion transformQuaternion(
+  const geometry_msgs::msg::Quaternion & quat, const geometry_msgs::msg::TransformStamped & transform)
+{
+  geometry_msgs::msg::QuaternionStamped quat_stamped;
+  quat_stamped.quaternion = quat;
+
+  geometry_msgs::msg::QuaternionStamped quat_stamped_transformed;
+  tf2::doTransform(quat_stamped, quat_stamped_transformed, transform);
+  return quat_stamped_transformed.quaternion;
+}
+
 namespace imu_corrector
 {
 ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & node_options)
@@ -103,6 +114,7 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m
   sensor_msgs::msg::Imu imu_msg_base_link;
   imu_msg_base_link.header.stamp = imu_msg_ptr->header.stamp;
   imu_msg_base_link.header.frame_id = output_frame_;
+  imu_msg_base_link.orientation = transformQuaternion(imu_msg.orientation, *tf_imu2base_ptr); 
   imu_msg_base_link.linear_acceleration =
     transformVector3(imu_msg.linear_acceleration, *tf_imu2base_ptr);
   imu_msg_base_link.linear_acceleration_covariance =

diff --git a/...ace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml b/...ace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
@@ -2,10 +2,23 @@
 <launch>
     <arg name="rviz_config" default="$(find-pkg-share aichallenge_system_launch)/config/autoware.rviz" description="rviz config"/>
     <log message="This is aichallenge_system_launch."/>
-    <include file="$(find-pkg-share aichallenge_submit_launch)/launch/aichallenge_submit.launch.xml" />
+    <arg name="simulation" default="true"/>
+    <arg name="use_sim_time" default="$(var simulation)"/>
+    <arg name="launch_vehicle_interface" default="false"/>
+    <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)"/>
 
+    <include file="$(find-pkg-share aichallenge_submit_launch)/launch/aichallenge_submit.launch.xml" >
+        <arg name="sensor_model" value="$(var sensor_model)"/>
+        <arg name="use_sim_time" value="$(var use_sim_time)"/>
+        <arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
+    </include>
+
     <!-- Workaround because the simulator cannot use the service. -->
-    <node pkg="aichallenge_system_launch" exec="control_mode_adapter.py" output="screen"/>
+    <group if="$(var simulation)">
+        <node pkg="aichallenge_system_launch" exec="control_mode_adapter.py" output="screen"/>
+    </group>
 
     <!-- RViz -->
     <group>