add mppi_h node

MizuhoAOKI · Dec 10, 2024 · 375ac62 · 375ac62
1 parent 50c5ffd
commit 375ac62
Show file tree

Hide file tree

Showing 24 changed files with 3,309 additions and 10 deletions.
diff --git a/README.md b/README.md
@@ -160,15 +160,9 @@ roslaunch launch/gazebo_world.launch gazebo_world_name:=maze
     source /opt/ros/noetic/setup.bash && source ./devel/setup.bash
     roslaunch launch/navigation.launch local_planner:=mppi_4d
     ```
-
-> [!NOTE]
-> MPPI-H are under preparation. please wait for the update.
-
-<!--
-- ✨Try MPPI-H✨ (good balance between quickness and safety, recommended)
+- [Author's Recommendation] ✨Try MPPI-H✨ (good balance between quickness and safety)
     ```bash
     cd <path to your workspace>/mppi_swerve_drive_ros
     source /opt/ros/noetic/setup.bash && source ./devel/setup.bash
     roslaunch launch/navigation.launch local_planner:=mppi_h
     ```
--->
diff --git a/launch/navigation.launch b/launch/navigation.launch
@@ -5,7 +5,7 @@
   <!-- often changed arguments -->
   <arg name="gazebo_world_name" default="maze" /> <!-- empty, empty_garden, cylinder_garden, maze -->
   <arg name="joy_operation" default="true" />
-  <arg name="local_planner" default="mppi_3d_b" /> <!-- mppi_3d_a, mppi_3d_b, mppi_4d --> <!-- TODO: mppi_h will come soon. -->
+  <arg name="local_planner" default="mppi_h" /> <!-- mppi_3d_a, mppi_3d_b, mppi_4d, mppi_h -->
 
   <!-- arguments -->
   <arg name="workspace" default="$(env HOME)/mppi_swerve_drive_ros" />
@@ -133,9 +133,9 @@
       <include file="$(find vel_driver)/launch/vel_driver.launch" />
   </group>
 
-  <!-- MPPI-H (switching MPPI-3D and MPPI-4D in real-time) --> <!-- TODO: Coming Soon -->
+  <!-- MPPI-H (switching MPPI-3D and MPPI-4D in real-time) -->
   <group if="$(eval local_planner=='mppi_h')">
-      <include file="$(find hybrid_mppi)/launch/mppi_h.launch" />
+      <include file="$(find mppi_h)/launch/mppi_h.launch" />
 
       <!-- reference_costmap_generator -->
       <include file="$(find reference_costmap_generator)/launch/reference_costmap_generator.launch" />

diff --git a/src/control/mppi_h/CMakeLists.txt b/src/control/mppi_h/CMakeLists.txt
@@ -0,0 +1,102 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mppi_h)
+
+# parameters
+option(USE_OPENMP "USE_OPENMP" ON) # ON / OFF
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  visualization_msgs
+  jsk_rviz_plugins
+  nav_msgs
+  roscpp
+  tf2
+  tf2_geometry_msgs
+  tf2_ros
+  grid_map_core
+  grid_map_ros
+  grid_map_filters
+  grid_map_loader
+  grid_map_msgs
+  grid_map_rviz_plugin
+  grid_map_visualization
+  mppi_eval_msgs
+)
+
+# use eigen3 (matrix computation library)
+find_package(Eigen3 REQUIRED)
+
+# load openmp
+if(USE_OPENMP)
+  find_package(OpenMP REQUIRED)
+  if(OpenMP_FOUND)
+      message(WARNING "OpenMP found. Activate CPU acceleration.")
+      set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+      set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+  endif()
+  if(NOT OPENMP_FOUND)
+      message(FATAL ERROR "Unable to find OpenMP library.")
+  endif()
+endif()
+
+catkin_package(
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+## Declare a C++ library
+add_library(mppi_h_core SHARED src/mppi_h_core.cpp)
+add_library(mppi_mode1_core SHARED src/mode1_mppi_3d/mppi_3d_core.cpp)
+add_library(mppi_mode2_core SHARED src/mode2_mppi_4d/mppi_4d_core.cpp)
+
+## link openmp
+if(USE_OPENMP)
+  if (OPENMP_FOUND)
+      if (TARGET OpenMP::OpenMP_CXX)
+          target_link_libraries(mppi_mode1_core OpenMP::OpenMP_CXX)
+          target_link_libraries(mppi_mode2_core OpenMP::OpenMP_CXX)
+      endif()
+  endif()
+endif()
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/mppi_h_node.cpp src/mppi_h.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+  ${catkin_LIBRARIES}
+  mppi_h_core
+  mppi_mode1_core
+  mppi_mode2_core
+)
+
+#############
+## Install ##
+#############
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+install(
+  DIRECTORY
+    launch
+    config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/src/control/mppi_h/README.md b/src/control/mppi_h/README.md
@@ -0,0 +1,131 @@
+# mppi_h
+
+MPPI-H local planner for autonomous navigation of a 4wids vehicle.
+MPPI-H switches between MPPI-3D and MPPI-4D controllers depending on the situation in real-time.
+
+## Subscribing Topics
+
+| Topic name      | Type                 | Description                                                  |
+| --------------- | -------------------- | ------------------------------------------------------------ |
+| /groundtruth_odom | nav_msgs/Odometry | The ground truth odometry data. |
+| /move_base/NavfnROS/plan | nav_msgs/Path | The global path data. |
+| /move_base/local_costmap/costmap | nav_msgs/OccupancyGrid | The local costmap data. |
+| /distance_error_map | grid_map_msgs/GridMap | The distance error map data. |
+| /ref_yaw_map | grid_map_msgs/GridMap | The reference yaw map data. |
+
+## Publishing Topics
+
+| Topic name      | Type              | Description                                                  |
+| --------------- | ----------------- | ------------------------------------------------------------ |
+| /cmd_vel | geometry_msgs/Twist | The velocity command consisting of linear and angular velocities (vx, vy, omega). |
+| /mppi/cmd/absvel | geometry_msgs/Twist | The absolute velocity command. i.e. absvel = sqrt(vx^2 + vy^2). |
+| /mppi/cmd/vx | std_msgs/Float32 | The linear velocity command. The front direction of the vehicle is positive. |
+| /mppi/cmd/vy | std_msgs/Float32 | The lateral velocity command. The left direction of the vehicle is positive. |
+| /mppi/cmd/omega | std_msgs/Float32 | The angular velocity command. The counter-clockwise direction is positive. |
+| /mppi/calc_time | std_msgs/Float32 | The calculation time of the MPPI controller. |
+| /mppi/overlay_text | std_msgs/String | The overlay text to show mppi name on rviz.|
+| /mppi/optimal_traj | visualization_msgs/MarkerArray | The optimal trajectory for visualization. |
+| /mppi/sampled_traj | visualization_msgs/MarkerArray | The sampled trajectory for visualization. |
+| /mppi/eval_info | mppi_eval_msgs/MPPIEval | The evaluation information of the MPPI controller. |
+
+## Node Parameters
+
+### Common Parameters
+
+| Parameter name               | Type   | Description                                                  |
+| ---------------------------- | ------ | ------------------------------------------------------------ |
+| /mppi_h/navigation/xy_goal_tolerance | double | The tolerance [m] to judge that the goal is reached. |
+| /mppi_h/navigation/yaw_goal_tolerance | double | The tolerance [rad] to judge that the goal is reached. |
+| /mppi_h/target_system/d_l | double | The distance [m] from the center of the vehicle to the left wheel. |
+| /mppi_h/target_system/d_r | double | The distance [m] from the center of the vehicle to the right wheel. |
+| /mppi_h/target_system/l_f | double | The distance [m] from the center of the vehicle to the front axle. |
+| /mppi_h/target_system/l_r | double | The distance [m] from the center of the vehicle to the rear axle. |
+| /mppi_h/target_system/tire_radius | double | The radius [m] of the tire. |
+| /mppi_h/controller/common/control_interval | double | The control interval [s]. |
+| /mppi_h/controller/common/num_samples | int | The number of samples [samples]. |
+| /mppi_h/controller/common/prediction_horizon | int | The prediction horizon [steps]. |
+| /mppi_h/controller/common/step_len_sec | double | The step length [s] in the prediction horizon. |
+| /mppi_h/controller/mode1/param_exploration | double | The exploration parameter of MPPI. |
+| /mppi/mode_selector/yaw_error_threshold | double | The yaw error threshold [rad] to switch between MPPI-3D and MPPI-4D. |
+| /mppi/mode_selector/dist_error_threshold | double | The distance error threshold [m] to switch between MPPI-3D and MPPI-4D. |
+
+### Parameters of Mode 1 Controller (MPPI-3D)
+
+| Parameter name               | Type   | Description                                                  |
+| ---------------------------- | ------ | ------------------------------------------------------------ |
+| /mppi_h/controller/mode1/param_lambda | double | The lambda parameter of MPPI. |
+| /mppi_h/controller/mode1/param_alpha | double | The alpha parameter of MPPI. |
+| /mppi_h/controller/mode1/sigma | double[3] | The standard deviation of the noise for [vx, vy, omega]. |
+| /mppi_h/controller/mode1/reduce_computation | bool | If true, noise sampling is done only once and the same noise is used for all processes. |
+| /mppi_h/controller/mode1/weight_cmd_change | double[3] | The penalty weight for the variation of [vx, vy, omega]. |
+| /mppi_h/controller/mode1/weight_vehicle_cmd_change | double[8] | The penalty weight for the variation of [fl_steer, fr_steer, rl_steer, rr_steer, fl_vel, fr_vel, rl_vel, rr_vel]. |
+| /mppi_h/controller/mode1/reference_velocity | double | The reference velocity [m/s]. |
+| /mppi_h/controller/mode1/weight_velocity_error | double | The penalty weight for the velocity error. |
+| /mppi_h/controller/mode1/weight_angular_error | double | The penalty weight for the angular error. |
+| /mppi_h/controller/mode1/weight_collision_penalty | double | The penalty weight for the collision. |
+| /mppi_h/controller/mode1/weight_distance_error_penalty | double | The penalty weight for the distance error. |
+| /mppi_h/controller/mode1/weight_terminal_state_penalty | double | The penalty weight for the terminal state. |
+| /mppi_h/controller/mode1/use_sg_filter | bool | If true, Savitzky-Golay filter is used for smoothing the control input. |
+| /mppi_h/controller/mode1/sg_filter_half_window_size | int | The half window size of the Savitzky-Golay filter. |
+| /mppi_h/controller/mode1/sg_filter_poly_order | int | The polynomial order of the Savitzky-Golay filter. |
+
+### Parameters of Mode 2 Controller (MPPI-4D)
+
+| Parameter name               | Type   | Description                                                  |
+| ---------------------------- | ------ | ------------------------------------------------------------ |
+| /mppi_h/controller/mode2/param_lambda | double | The lambda parameter of MPPI. |
+| /mppi_h/controller/mode2/param_alpha | double | The alpha parameter of MPPI. |
+| /mppi_h/controller/mode2/sigma | double[4] | The standard deviation of the noise for [vx, vy, omega, steer]. |
+| /mppi_h/controller/mode2/reduce_computation | bool | If true, noise sampling is done only once and the same noise is used for all processes. |
+| /mppi_h/controller/mode2/weight_cmd_change | double[4] | The penalty weight for the variation of [vx, vy, omega, steer]. |
+| /mppi_h/controller/mode2/weight_vehicle_cmd_change | double[8] | The penalty weight for the variation of [fl_steer, fr_steer, rl_steer, rr_steer, fl_vel, fr_vel, rl_vel, rr_vel]. |
+| /mppi_h/controller/mode2/reference_velocity | double | The reference velocity [m/s]. |
+| /mppi_h/controller/mode2/weight_velocity_error | double | The penalty weight for the velocity error. |
+| /mppi_h/controller/mode2/weight_angular_error | double | The penalty weight for the angular error. |
+| /mppi_h/controller/mode2/weight_collision_penalty | double | The penalty weight for the collision. |
+| /mppi_h/controller/mode2/weight_distance_error_penalty | double | The penalty weight for the distance error. |
+| /mppi_h/controller/mode2/weight_terminal_state_penalty | double | The penalty weight for the terminal state. |
+| /mppi_h/controller/mode2/use_sg_filter | bool | If true, Savitzky-Golay filter is used for smoothing the control input. |
+| /mppi_h/controller/mode2/sg_filter_half_window_size | int | The half window size of the Savitzky-Golay filter. |
+| /mppi_h/controller/mode2/sg_filter_poly_order | int | The polynomial order of the Savitzky-Golay filter. |
+
+## Usage
+
+To launch the node individually, run the following commands.
+
+For mppi_h node:
+```
+source devel/setup.bash
+roslaunch mppi_h mppi_h.launch
+```
+
+## Note
+
+`rostopic echo /mppi/eval_info` will show you the evaluation information of the MPPI controller in real-time.
+An example output is shown below.
+
+```
+header: 
+  seq: 126
+  stamp: 
+    secs: 13
+    nsecs: 660000000
+  frame_id: "mppi_3d_b"
+state_cost: 1134.80859375
+global_x: 6.007351875305176
+global_y: 2.598604202270508
+global_yaw: 0.27291983366012573
+cmd_vx: 1.464385747909546
+cmd_vy: -0.19296832382678986
+cmd_yawrate: 0.28070420026779175
+cmd_steer_fl: -0.03971844166517258
+cmd_steer_fr: -0.03277630731463432
+cmd_steer_rl: -0.24662145972251892
+cmd_steer_rr: -0.20479810237884521
+cmd_rotor_fl: 6.625393390655518
+cmd_rotor_fr: 8.028000831604004
+cmd_rotor_rl: 6.826725959777832
+cmd_rotor_rr: 8.1949462890625
+calc_time_ms: 3.0
+goal_reached: False
+```
diff --git a/src/control/mppi_h/config/mppi_h.yaml b/src/control/mppi_h/config/mppi_h.yaml
@@ -0,0 +1,83 @@
+# topic names
+## subscribing topics
+odom_topic: /groundtruth_odom
+ref_path_topic: /move_base/NavfnROS/plan
+collision_costmap_topic: /move_base/local_costmap/costmap
+distance_error_map_topic: /distance_error_map
+ref_yaw_map_topic: /ref_yaw_map
+## publishing topics
+control_cmd_vel_topic: /cmd_vel
+mppi_absvel_topic: /mppi/cmd/absvel
+mppi_vx_topic: /mppi/cmd/vx
+mppi_vy_topic: /mppi/cmd/vy
+mppi_omega_topic: /mppi/cmd/omega
+calc_time_topic: /mppi/calc_time
+mppi_overlay_text_topic: /mppi/overlay_text
+mppi_optimal_traj_topic: /mppi/optimal_traj
+mppi_sampled_traj_topic: /mppi/sampled_traj
+mppi_eval_msg_topic: /mppi/eval_info
+
+# navigation params
+navigation:
+  xy_goal_tolerance: 0.5 # [m]
+  yaw_goal_tolerance: 3.14 # [rad]
+
+# target_system params
+target_system:
+  l_f: 0.5 # [m]
+  l_r: 0.5 # [m]
+  d_l: 0.5 # [m]
+  d_r: 0.5 # [m]
+  tire_radius: 0.2 # [m]
+
+# mode selector params
+mode_selector:
+  yaw_error_threshold: 0.5 # [rad]
+  dist_error_threshold: 0.5 # [m]
+
+# controller params
+controller:
+  common: # common parameters
+    control_interval: 0.05 # [s]
+    prediction_horizon: 30 # [steps]
+    step_len_sec: 0.033 # [s]
+
+  mode1: # mode one: mppi_3d
+    name: "[mode 1] mppi_3d_b"
+    num_samples: 3000 # number of samples
+    param_exploration: 0.1
+    param_lambda: 200.0
+    param_alpha: 0.975
+    sigma: [0.55, 0.55, 0.96] # noise for [vx, vy, omega]
+    reduce_computation: false # if true, noise sampling is done only once and the same noise is used for all processes.
+    weight_cmd_change: [0.0, 0.0, 0.0] # penalty weight for variation of [vx, vy, omega]
+    weight_vehicle_cmd_change: [1.4, 1.4, 1.4, 1.4, 0.1, 0.1, 0.1, 0.1] # penalty weight for variation of [fl_steer, fr_steer, rl_steer, rr_steer, fl_vel, fr_vel, rl_vel, rr_vel]
+    reference_velocity: 2.0 # [m/s]
+    weight_velocity_error: 10.0
+    weight_angular_error: 30.0
+    weight_collision_penalty: 50.0
+    weight_distance_error_penalty: 40.0
+    weight_terminal_state_penalty: 10.0
+    use_sg_filter: true # set true to use Savitzky-Golay filter for smoothing the control input
+    sg_filter_half_window_size: 4 # value in the range of 1 ~ (prediction_horizon - 1) is allowed.
+    sg_filter_poly_order: 6
+
+  mode2: # mode two: mppi_4d
+    name: "[mode 2] mppi_4d"
+    num_samples: 3000 # number of samples
+    param_exploration: 0.1
+    param_lambda: 100.0
+    param_alpha: 0.975
+    sigma: [0.78, 0.78, 1.00, 1.00] # noise for [fl_steer, rr_steer, fl_vel, rr_vel]
+    reduce_computation: false # if true, noise sampling is done only once and the same noise is used for all processes.
+    weight_cmd_change: [0.0, 0.0, 0.0] # penalty weight for variation of [vx, vy, omega]
+    weight_vehicle_cmd_change: [1.4, 1.4, 1.4, 1.4, 0.1, 0.1, 0.1, 0.1] # penalty weight for variation of [fl_steer, fr_steer, rl_steer, rr_steer, fl_vel, fr_vel, rl_vel, rr_vel]
+    reference_velocity: 2.0 # [m/s]
+    weight_velocity_error: 10.0
+    weight_angular_error: 30.0
+    weight_collision_penalty: 50.0
+    weight_distance_error_penalty: 40.0
+    weight_terminal_state_penalty: 10.0
+    use_sg_filter: true # set true to use Savitzky-Golay filter for smoothing the control input
+    sg_filter_half_window_size: 4 # value in the range of 1 ~ (prediction_horizon - 1) is allowed.
+    sg_filter_poly_order: 6