update simulator

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py

@@ -54,11 +54,14 @@ def launch_setup(context, *args, **kwargs):
         ],
         remappings=[
             ("input/ackermann_control_command", "/control/command/control_cmd"),
+            ("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
             ("input/gear_command", "/control/command/gear_cmd"),
+            ("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
             ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
             ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
             ("input/trajectory", "/planning/scenario_planning/trajectory"),
             ("input/engage", "/vehicle/engage"),
+            ("input/control_mode_request", "/system/control_mode_request"),
             ("output/twist", "/vehicle/status/velocity_status"),
             ("output/odometry", "/localization/kinematic_state"),
             ("output/steering", "/vehicle/status/steering_status"),

simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

@@ -112,7 +112,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
     "input/control_mode_request",
     std::bind(&SimplePlanningSimulator::on_control_mode_request, this, _1, _2));
 
-  // TODO(Horibe): will be replaced by mode_request. May still be needed for scenario testing
+  // TODO(Horibe): should be replaced by mode_request. Keep for the backward compatibility.
   sub_engage_ = create_subscription<Engage>(
     "input/engage", rclcpp::QoS{1}, std::bind(&SimplePlanningSimulator::on_engage, this, _1));
 
@@ -178,6 +178,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
 
   // control mode
   current_control_mode_.data = ControlMode::AUTO;
+  current_manual_gear_cmd_.command = GearCommand::DRIVE;
 }
 
 void SimplePlanningSimulator::initialize_vehicle_model()