diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/CMakeLists.txt
index 67b1d203..198d0c57 100644
--- 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/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/converter.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/converter.param.yaml
new file mode 100644
index 00000000..3e98b794
--- /dev/null
+++ b/aichallenge/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/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/imu_corrector.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/imu_corrector.param.yaml
new file mode 100644
index 00000000..3156598f
--- /dev/null
+++ b/aichallenge/workspace/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/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/config/lanelet2_map_loader.param.yaml
new file mode 100755
index 00000000..61c41ee8
--- /dev/null
+++ b/aichallenge/workspace/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
new file mode 100644
index 00000000..32e639ca
--- /dev/null
+++ 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
new file mode 100644
index 00000000..adf2c809
--- /dev/null
+++ 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
new file mode 100644
index 00000000..3da0e0cd
--- /dev/null
+++ 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/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
index 8e2fe222..ac984457 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -68,6 +68,7 @@
+
@@ -250,6 +251,26 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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
index b19c1bde..bc2a50bf 100644
--- 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
index edc82f6d..5c84d470 100644
--- 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/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
index 9969163f..9de12220 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/aichallenge_system.launch.xml
@@ -2,10 +2,23 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
-
+
+
+