diff --git a/.cspell.json b/.cspell.json
index 3c1715b8..7978a84c 100644
--- a/.cspell.json
+++ b/.cspell.json
@@ -104,6 +104,28 @@
"wpedantic",
"xacro",
"xyzrpy",
- "zcvf"
+ "zcvf",
+ "mppi",
+ "Qdin",
+ "Kohei",
+ "MPPI",
+ "Savitzky",
+ "satitzky",
+ "Golay",
+ "Vandermonde",
+ "michikuni",
+ "eguchi",
+ "vander",
+ "pinv",
+ "dtheta",
+ "cind",
+ "dind",
+ "ncourse",
+ "argmin",
+ "reparameting",
+ "tempature",
+ "rsample",
+ "coeffs",
+ "softplus"
]
}
diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS
index 379992ee..13913d57 100644
--- a/.github/CODEOWNERS
+++ b/.github/CODEOWNERS
@@ -12,3 +12,4 @@ aichallenge/workspace/src/aichallenge_submit/racing_kart_description/** @Autumn6
aichallenge/workspace/src/aichallenge_submit/racing_kart_sensor_kit_description/** @booars/aic2024-developers
aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/** @sitahara
aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/** @booars/aic2024-developers
+aichallenge/workspace/src/aichallenge_submit/mppi_controller/** @tamago117
diff --git a/aichallenge/pkill_all.sh b/aichallenge/pkill_all.sh
old mode 100644
new mode 100755
diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml
new file mode 100644
index 00000000..c5c54ba6
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/config/planning/costmap_generator.param.yaml
@@ -0,0 +1,18 @@
+/**:
+ ros__parameters:
+ update_rate: 10.0
+ map_frame_id: "map"
+ costmap_center_frame_id: "base_link"
+ costmap_width: 50.0
+ costmap_resolution: 0.2
+ multi_layered_costmap:
+ layers:
+ - "lanelet_layer"
+ - "predicted_object_layer"
+ lanelet_layer:
+ type: "lanelet"
+ map_topic: "/map/vector_map"
+ predicted_object_layer:
+ type: "predicted_object"
+ predicted_objects_topic: "/perception/object_recognition/objects"
+ distance_threshold: 1.2
diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml
index f23f0812..d39ec7b6 100644
--- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/control.launch.xml
@@ -5,12 +5,13 @@
-
-
+
+
-
+
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml
index 2c8c989f..24425f1b 100644
--- a/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/booars_launch/launch/components/planning.launch.xml
@@ -100,5 +100,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml
index 9501d94e..0dea8897 100644
--- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/launch/costmap_generator.launch.xml
@@ -1,7 +1,10 @@
+
+
+
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore
new file mode 100644
index 00000000..7e99e367
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/.gitignore
@@ -0,0 +1 @@
+*.pyc
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml
new file mode 100644
index 00000000..8f0d35a0
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/config/mppi_controller.param.yaml
@@ -0,0 +1,26 @@
+/**:
+ ros__parameters:
+ # mppi
+ horizon : 25
+ num_samples : 3000
+ u_min : [-2.0, -0.25] # accel(m/s2), steer angle(rad)
+ u_max : [2.0, 0.25]
+ sigmas : [0.5, 0.15] # sample range
+ lambda : 1.0
+ auto_lambda : false
+ # reference path
+ DL : 0.1
+ lookahead_distance : 0.3
+ reference_path_interval : 0.85
+ # cost weights
+ Qc : 20.0
+ Ql : 1.0
+ Qv : 2.0
+ Qo : 10000.0
+ Qin : 0.01
+ Qdin : 0.5
+ # model param
+ delta_t : 0.1
+ vehicle_L : 1.0
+ V_MAX : 8.0
+
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py
new file mode 100644
index 00000000..90f52793
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/MPPI.py
@@ -0,0 +1,463 @@
+"""
+Kohei Honda, 2023.
+"""
+
+from __future__ import annotations
+
+from typing import Callable, Tuple, Dict
+import torch
+import torch.nn as nn
+from torch.distributions.multivariate_normal import MultivariateNormal
+
+
+class MPPI(nn.Module):
+ """
+ Model Predictive Path Integral Control,
+ J. Williams et al., T-RO, 2017.
+ """
+
+ def __init__(
+ self,
+ horizon: int,
+ num_samples: int,
+ dim_state: int,
+ dim_control: int,
+ dynamics: Callable[[torch.Tensor, torch.Tensor], torch.Tensor],
+ cost_func: Callable[[torch.Tensor, torch.Tensor, Dict], torch.Tensor],
+ u_min: torch.Tensor,
+ u_max: torch.Tensor,
+ sigmas: torch.Tensor,
+ lambda_: float,
+ auto_lambda: bool = False,
+ exploration: float = 0.0,
+ use_sg_filter: bool = False,
+ sg_window_size: int = 5,
+ sg_poly_order: int = 3,
+ device=torch.device("cuda"),
+ dtype=torch.float32,
+ seed: int = 42,
+ ) -> None:
+ """
+ :param horizon: Predictive horizon length.
+ :param predictive_interval: Predictive interval (seconds).
+ :param delta: predictive horizon step size (seconds).
+ :param num_samples: Number of samples.
+ :param dim_state: Dimension of state.
+ :param dim_control: Dimension of control.
+ :param dynamics: Dynamics model.
+ :param cost_func: Cost function.
+ :param u_min: Minimum control.
+ :param u_max: Maximum control.
+ :param sigmas: Noise standard deviation for each control dimension.
+ :param lambda_: temperature parameter.
+ :param exploration: Exploration rate when sampling.
+ :param use_sg_filter: Use Savitzky-Golay filter.
+ :param sg_window_size: Window size for Savitzky-Golay filter. larger is smoother. Must be odd.
+ :param sg_poly_order: Polynomial order for Savitzky-Golay filter. Smaller is smoother.
+ :param device: Device to run the solver.
+ :param dtype: Data type to run the solver.
+ :param seed: Seed for torch.
+ """
+
+ super().__init__()
+
+ # torch seed
+ torch.manual_seed(seed)
+
+ # check dimensions
+ assert u_min.shape == (dim_control,)
+ assert u_max.shape == (dim_control,)
+ assert sigmas.shape == (dim_control,)
+ # assert num_samples % batch_size == 0 and num_samples >= batch_size
+
+ # device and dtype
+ if torch.cuda.is_available() and device == torch.device("cuda"):
+ self._device = torch.device("cuda")
+ else:
+ self._device = torch.device("cpu")
+ print(f"Device: {self._device}")
+ self._dtype = dtype
+
+ # set parameters
+ self._horizon = horizon
+ self._num_samples = num_samples
+ self._dim_state = dim_state
+ self._dim_control = dim_control
+ self._dynamics = dynamics
+ self._cost_func = cost_func
+ self._u_min = u_min.clone().detach().to(self._device, self._dtype)
+ self._u_max = u_max.clone().detach().to(self._device, self._dtype)
+ self._sigmas = sigmas.clone().detach().to(self._device, self._dtype)
+ self._lambda = lambda_
+ self._exploration = exploration
+ self._use_sg_filter = use_sg_filter
+ self._sg_window_size = sg_window_size
+ self._sg_poly_order = sg_poly_order
+
+ # noise distribution
+ self._covariance = torch.zeros(
+ self._horizon,
+ self._dim_control,
+ self._dim_control,
+ device=self._device,
+ dtype=self._dtype,
+ )
+ self._covariance[:, :, :] = torch.diag(sigmas**2).to(self._device, self._dtype)
+ self._inv_covariance = torch.zeros_like(
+ self._covariance, device=self._device, dtype=self._dtype
+ )
+ for t in range(1, self._horizon):
+ self._inv_covariance[t] = torch.inverse(self._covariance[t])
+
+ zero_mean = torch.zeros(dim_control, device=self._device, dtype=self._dtype)
+ self._noise_distribution = MultivariateNormal(
+ loc=zero_mean, covariance_matrix=self._covariance
+ )
+
+ self._sample_shape = torch.Size([self._num_samples])
+
+ # sampling with reparameting trick
+ self._action_noises = self._noise_distribution.rsample(
+ sample_shape=self._sample_shape
+ )
+
+ zero_mean_seq = torch.zeros(
+ self._horizon, self._dim_control, device=self._device, dtype=self._dtype
+ )
+ self._perturbed_action_seqs = torch.clamp(
+ zero_mean_seq + self._action_noises, self._u_min, self._u_max
+ )
+
+ self._previous_action_seq = zero_mean_seq
+
+ # init satitzky-golay filter
+ self._coeffs = self._savitzky_golay_coeffs(
+ self._sg_window_size, self._sg_poly_order
+ )
+ self._actions_history_for_sg = torch.zeros(
+ self._horizon - 1, self._dim_control, device=self._device, dtype=self._dtype
+ ) # previous inputted actions for sg filter
+
+ # inner variables
+ self._state_seq_batch = torch.zeros(
+ self._num_samples,
+ self._horizon + 1,
+ self._dim_state,
+ device=self._device,
+ dtype=self._dtype,
+ )
+ self._weights = torch.zeros(
+ self._num_samples, device=self._device, dtype=self._dtype
+ )
+ self._optimal_state_seq = torch.zeros(
+ self._horizon + 1, self._dim_state, device=self._device, dtype=self._dtype
+ )
+
+ # auto lambda tuning
+ self._auto_lambda = auto_lambda
+ if auto_lambda:
+ self.log_tempature = torch.nn.Parameter(
+ torch.log(
+ torch.tensor([self._lambda], device=self._device, dtype=self._dtype)
+ )
+ )
+ self.optimizer = torch.optim.Adam([self.log_tempature], lr=1e-2)
+
+ def reset(self):
+ """
+ Reset the previous action sequence.
+ """
+ self._previous_action_seq = torch.zeros(
+ self._horizon, self._dim_control, device=self._device, dtype=self._dtype
+ )
+ self._actions_history_for_sg = torch.zeros(
+ self._horizon - 1, self._dim_control, device=self._device, dtype=self._dtype
+ ) # previous inputted actions for sg filter
+
+ def forward(
+ self, state: torch.Tensor, info: Dict = {}
+ ) -> Tuple[torch.Tensor, torch.Tensor]:
+ """
+ Solve the optimal control problem.
+ Args:
+ state (torch.Tensor): Current state.
+ Returns:
+ Tuple[torch.Tensor, torch.Tensor]: Tuple of predictive control and state sequence.
+ """
+ assert state.shape == (self._dim_state,)
+
+ if not torch.is_tensor(state):
+ state = torch.tensor(state, device=self._device, dtype=self._dtype)
+ else:
+ if state.device != self._device or state.dtype != self._dtype:
+ state = state.to(self._device, self._dtype)
+
+ mean_action_seq = self._previous_action_seq.clone().detach()
+
+ # random sampling with reparametrization trick
+ self._action_noises = self._noise_distribution.rsample(
+ sample_shape=self._sample_shape
+ )
+
+ # noise injection with exploration
+ threshold = int(self._num_samples * (1 - self._exploration))
+ inherited_samples = mean_action_seq + self._action_noises[:threshold]
+ self._perturbed_action_seqs = torch.cat(
+ [inherited_samples, self._action_noises[threshold:]]
+ )
+
+ # clamp actions
+ self._perturbed_action_seqs = torch.clamp(
+ self._perturbed_action_seqs, self._u_min, self._u_max
+ )
+
+ # rollout samples in parallel
+ self._state_seq_batch[:, 0, :] = state.repeat(self._num_samples, 1)
+
+ for t in range(self._horizon):
+ self._state_seq_batch[:, t + 1, :] = self._dynamics(
+ self._state_seq_batch[:, t, :],
+ self._perturbed_action_seqs[:, t, :],
+ )
+
+ # compute sample costs
+ costs = torch.zeros(
+ self._num_samples, self._horizon, device=self._device, dtype=self._dtype
+ )
+ action_costs = torch.zeros(
+ self._num_samples, self._horizon, device=self._device, dtype=self._dtype
+ )
+ initial_state = self._state_seq_batch[:, 0, :]
+ for t in range(self._horizon):
+ prev_index = t - 1 if t > 0 else 0
+ prev_state = self._state_seq_batch[:, prev_index, :]
+ prev_action = self._perturbed_action_seqs[:, prev_index, :]
+ # info update
+ info["prev_state"] = prev_state
+ info["prev_action"] = prev_action
+ info["initial_state"] = initial_state
+ info["t"] = t
+ costs[:, t] = self._cost_func(
+ self._state_seq_batch[:, t, :],
+ self._perturbed_action_seqs[:, t, :],
+ info,
+ )
+ action_costs[:, t] = (
+ mean_action_seq[t]
+ @ self._inv_covariance[t]
+ @ self._perturbed_action_seqs[:, t].T
+ )
+
+ prev_state = self._state_seq_batch[:, -2, :]
+ info["prev_state"] = prev_state
+ zero_action = torch.zeros(
+ self._num_samples,
+ self._dim_control,
+ device=self._device,
+ dtype=self._dtype,
+ )
+ terminal_costs = self._cost_func(
+ self._state_seq_batch[:, -1, :], zero_action, info
+ )
+
+ # In the original paper, the action cost is added to consider KL div. penalty,
+ # but it is easier to tune without it
+ costs = (
+ torch.sum(costs, dim=1)
+ + terminal_costs
+ # + torch.sum(self._lambda * action_costs, dim=1)
+ )
+
+ # calculate weights
+ self._weights = torch.softmax(-costs / self._lambda, dim=0)
+
+ # find optimal control by weighted average
+ optimal_action_seq = torch.sum(
+ self._weights.view(self._num_samples, 1, 1) * self._perturbed_action_seqs,
+ dim=0,
+ )
+
+ mean_action_seq = optimal_action_seq
+
+ # auto-tune temperature parameter
+ # Refer E step of MPO algorithm:
+ # https://arxiv.org/pdf/1806.06920
+ if self._auto_lambda:
+ for _ in range(1):
+ self.optimizer.zero_grad()
+ tempature = torch.nn.functional.softplus(self.log_tempature)
+ cost_logsumexp = torch.logsumexp(-costs / tempature, dim=0)
+ epsilon = 0.1 # tolerance hyperparameter for KL divergence
+ loss = tempature * (epsilon + torch.mean(cost_logsumexp))
+ loss.backward()
+ self.optimizer.step()
+ self._lambda = torch.exp(self.log_tempature).item()
+
+ # calculate new covariance
+ # https://arxiv.org/pdf/2104.00241
+ # covariance = torch.sum(
+ # self._weights.view(self._num_samples, 1, 1)
+ # * (self._perturbed_action_seqs - optimal_action_seq) ** 2,
+ # dim=0,
+ # ) # T x dim_control
+
+ # small_cov = 1e-6 * torch.eye(
+ # self._dim_control, device=self._device, dtype=self._dtype
+ # )
+ # self._covariance = torch.diag_embed(covariance) + small_cov
+
+ # for t in range(1, self._horizon):
+ # self._inv_covariance[t] = torch.inverse(self._covariance[t])
+ # zero_mean = torch.zeros(self._dim_control, device=self._device, dtype=self._dtype)
+ # self._noise_distribution = MultivariateNormal(
+ # loc=zero_mean, covariance_matrix=self._covariance
+ # )
+
+ if self._use_sg_filter:
+ # apply savitzky-golay filter to N-1 previous action history + N optimal action seq
+ prolonged_action_seq = torch.cat(
+ [
+ self._actions_history_for_sg,
+ optimal_action_seq,
+ ],
+ dim=0,
+ )
+
+ # apply sg filter for each control dimension
+ filtered_action_seq = torch.zeros_like(
+ prolonged_action_seq, device=self._device, dtype=self._dtype
+ )
+ for i in range(self._dim_control):
+ filtered_action_seq[:, i] = self._apply_savitzky_golay(
+ prolonged_action_seq[:, i], self._coeffs
+ )
+
+ # use only N step optimal action seq
+ optimal_action_seq = filtered_action_seq[-self._horizon :]
+
+ # predictive state seq
+ expanded_optimal_action_seq = optimal_action_seq.repeat(1, 1, 1)
+ optimal_state_seq = self._states_prediction(state, expanded_optimal_action_seq)
+
+ # update previous actions
+ self._previous_action_seq = optimal_action_seq
+
+ # stuck previous actions for sg filter
+ optimal_action = optimal_action_seq[0]
+ self._actions_history_for_sg = torch.cat(
+ [self._actions_history_for_sg[1:], optimal_action.view(1, -1)]
+ )
+
+ return optimal_action_seq, optimal_state_seq
+
+ def get_top_samples(self, num_samples: int) -> Tuple[torch.Tensor, torch.Tensor]:
+ """
+ Get top samples.
+ Args:
+ num_samples (int): Number of state samples to get.
+ Returns:
+ Tuple[torch.Tensor, torch.Tensor]: Tuple of top samples and their weights.
+ """
+ assert num_samples <= self._num_samples
+
+ # large weights are better
+ top_indices = torch.topk(self._weights, num_samples).indices
+
+ top_samples = self._state_seq_batch[top_indices]
+ top_weights = self._weights[top_indices]
+
+ top_samples = top_samples[torch.argsort(top_weights, descending=True)]
+ top_weights = top_weights[torch.argsort(top_weights, descending=True)]
+
+ return top_samples, top_weights
+
+ def get_samples_from_posterior(
+ self, optimal_solution: torch.Tensor, state: torch.Tensor, num_samples: int
+ ) -> Tuple[torch.Tensor]:
+ assert num_samples <= self._num_samples
+
+ # posterior distribution of MPPI
+ # covariance is the same as noise distribution
+ posterior_distribution = MultivariateNormal(
+ loc=optimal_solution, covariance_matrix=self._covariance
+ )
+
+ # sampling control input sequence from posterior
+ samples = posterior_distribution.sample(sample_shape=torch.Size([num_samples]))
+
+ # get state sequence from sampled control input sequence
+ predictive_states = self._states_prediction(state, samples)
+
+ return samples, predictive_states
+
+ def _states_prediction(
+ self, state: torch.Tensor, action_seqs: torch.Tensor
+ ) -> torch.Tensor:
+ state_seqs = torch.zeros(
+ action_seqs.shape[0],
+ self._horizon + 1,
+ self._dim_state,
+ device=self._device,
+ dtype=self._dtype,
+ )
+ state_seqs[:, 0, :] = state
+ # expanded_optimal_action_seq = action_seq.repeat(1, 1, 1)
+ for t in range(self._horizon):
+ state_seqs[:, t + 1, :] = self._dynamics(
+ state_seqs[:, t, :], action_seqs[:, t, :]
+ )
+ return state_seqs
+
+ def _savitzky_golay_coeffs(self, window_size: int, poly_order: int) -> torch.Tensor:
+ """
+ Compute the Savitzky-Golay filter coefficients using PyTorch.
+
+ Parameters:
+ - window_size: The size of the window (must be odd).
+ - poly_order: The order of the polynomial to fit.
+
+ Returns:
+ - coeffs: The filter coefficients as a PyTorch tensor.
+ """
+ # Ensure the window size is odd and greater than the polynomial order
+ if window_size % 2 == 0 or window_size <= poly_order:
+ raise ValueError("window_size must be odd and greater than poly_order.")
+
+ # Generate the Vandermonde matrix of powers for the polynomial fit
+ half_window = (window_size - 1) // 2
+ indices = torch.arange(
+ -half_window, half_window + 1, dtype=self._dtype, device=self._device
+ )
+ A = torch.vander(indices, N=poly_order + 1, increasing=True)
+
+ # Compute the pseudo-inverse of the matrix
+ pseudo_inverse = torch.linalg.pinv(A)
+
+ # The filter coefficients are given by the first row of the pseudo-inverse
+ coeffs = pseudo_inverse[0]
+
+ return coeffs
+
+ def _apply_savitzky_golay(
+ self, y: torch.Tensor, coeffs: torch.Tensor
+ ) -> torch.Tensor:
+ """
+ Apply the Savitzky-Golay filter to a 1D signal using the provided coefficients.
+
+ Parameters:
+ - y: The input signal as a PyTorch tensor.
+ - coeffs: The filter coefficients as a PyTorch tensor.
+
+ Returns:
+ - y_filtered: The filtered signal.
+ """
+ # Pad the signal at both ends to handle the borders
+ pad_size = len(coeffs) // 2
+ y_padded = torch.cat([y[:pad_size].flip(0), y, y[-pad_size:].flip(0)])
+
+ # Apply convolution
+ y_filtered = torch.conv1d(
+ y_padded.view(1, 1, -1), coeffs.view(1, 1, -1), padding="valid"
+ )
+
+ return y_filtered.view(-1)
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py
new file mode 100644
index 00000000..747bc749
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/cost_map_tensor.py
@@ -0,0 +1,49 @@
+import torch
+from typing import Tuple
+
+class CostMapTensor:
+ def __init__(
+ self,
+ cost_map: torch.Tensor,
+ cell_size: float,
+ origin: Tuple[float, float] = (0.0, 0.0),
+ device=torch.device("cuda"),
+ dtype=torch.float32,
+ ) -> None:
+ """
+ cost map tensor for collision checking.
+ input:
+ cost_map (torch.Tensor): cost map tensor.
+ cell_size (float): size(m) of each cell in the cost map.
+ origin (Tuple[float, float]): origin of the cost map. (m, m)
+ device: device to run the computation.
+ dtype: data type of the tensor.
+ """
+ self.cost_map: torch.Tensor = cost_map
+ self.cell_size: float = cell_size
+ origin: Tuple[float, float] = origin
+ self.origin_tensor = torch.tensor(origin, device=device, dtype=dtype)
+ self.device = device
+ self.dtype = dtype
+
+ def compute_cost(self, x: torch.Tensor) -> torch.Tensor:
+ """
+ Check collision in a batch of trajectories.
+ :param x: Tensor of shape (batch_size, traj_length, position_dim).
+ :return: collision costs on the trajectories.
+ """
+ assert self.cost_map is not None
+ if x.device != self.device or x.dtype != self.dtype:
+ x = x.to(self.device, self.dtype)
+
+ # project to cell map
+ x_occ = (x - self.origin_tensor) / self.cell_size
+ x_occ = torch.round(x_occ).long().to(self.device)
+
+ x_occ[..., 0] = torch.clamp(x_occ[..., 0], 0, self.cost_map.shape[0] - 1)
+ x_occ[..., 1] = torch.clamp(x_occ[..., 1], 0, self.cost_map.shape[1] - 1)
+
+ # collision check
+ collisions = self.cost_map[x_occ[..., 1], x_occ[..., 0]]
+
+ return collisions
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py
new file mode 100644
index 00000000..7a65192f
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller.py
@@ -0,0 +1,244 @@
+import torch
+import numpy as np
+from typing import Tuple
+from mppi_controller.MPPI import MPPI
+from mppi_controller.cost_map_tensor import CostMapTensor
+
+import time
+
+@torch.jit.script
+def angle_normalize(x) -> torch.Tensor:
+ return ((x + torch.pi) % (2 * torch.pi)) - torch.pi
+
+class mppi_controller:
+ def __init__(self, config, debug=False, device=torch.device("cuda"), dtype=torch.float32):
+ self.config = config
+ self.debug = debug
+ # device and dtype
+ if torch.cuda.is_available() and device == torch.device("cuda"):
+ self._device = torch.device("cuda")
+ else:
+ self._device = torch.device("cpu")
+ self._dtype = dtype
+
+ self.u_min = torch.tensor(self.config["u_min"], device=self._device, dtype=self._dtype)
+ self.u_max = torch.tensor(self.config["u_max"], device=self._device, dtype=self._dtype)
+ self.sigmas = torch.tensor(self.config["sigmas"], device=self._device, dtype=self._dtype)
+
+ # solver
+ self.solver = MPPI(
+ horizon=self.config["horizon"],
+ num_samples=self.config["num_samples"],
+ dim_state=4,
+ dim_control=2,
+ dynamics=self.dynamics,
+ cost_func=self.cost_function,
+ u_min=self.u_min,
+ u_max=self.u_max,
+ sigmas=self.sigmas,
+ lambda_=self.config["lambda"],
+ auto_lambda=self.config["auto_lambda"],
+ )
+
+ # model parameter
+ self.delta_t = torch.tensor(self.config["delta_t"], device=self._device, dtype=self._dtype)
+ self.vehicle_L = torch.tensor(self.config["vehicle_L"], device=self._device, dtype=self._dtype)
+ self.V_MAX = torch.tensor(self.config["V_MAX"], device=self._device, dtype=self._dtype)
+
+ # cost weights
+ self.Qc = self.config["Qc"] # contouring error cost
+ self.Ql = self.config["Ql"] # lag error cost
+ self.Qv = self.config["Qv"] # velocity cost
+ self.Qo = self.config["Qo"] # obstacle cost
+ self.Qin = self.config["Qin"] # input cost
+ self.Qdin = self.config["Qdin"] # input rate cost
+
+ self.current_path_index = 0
+
+ # reference information (tensor)
+ self.reference_path: torch.Tensor = None
+ self.cost_map: CostMapTensor = None
+
+ def update(self, state: torch.Tensor, racing_center_path: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
+ """
+ Update the controller with the current state and reference path.
+ Args:
+ state (torch.Tensor): current state of the vehicle, shape (4,) [x, y, yaw, v]
+ racing_center_path (torch.Tensor): racing center path, shape (N, 3) [x, y, yaw]
+ Returns:
+ Tuple[torch.Tensor, torch.Tensor]: action sequence tensor, shape (horizon, 2) [accel, steer], state sequence tensor, shape (horizon + 1, 4) [x, y, yaw, v]
+ """
+
+ # reference
+ self.reference_path, self.current_path_index = self.calc_ref_trajectory(
+ state, racing_center_path, self.current_path_index, self.solver._horizon, DL=self.config["DL"], lookahead_distance=self.config["lookahead_distance"], reference_path_interval=self.config["reference_path_interval"]
+ )
+
+ if self.reference_path is None:
+ raise ValueError("reference path, obstacle map, and lane map must be set before calling solve method.")
+
+ # solve
+ start = time.time()
+ action_seq, state_seq = self.solver.forward(state=state)
+ end = time.time()
+ solve_time = end - start
+
+ if self.debug:
+ print("solve time: {}".format(round(solve_time * 1000, 2)), " [ms]")
+
+ return action_seq, state_seq
+
+ def get_top_samples(self, num_samples = 300) -> Tuple[torch.Tensor, torch.Tensor]:
+ return self.solver.get_top_samples(num_samples=num_samples)
+
+ def set_cost_map(self, cost_map: CostMapTensor) -> None:
+ self.cost_map = cost_map
+
+ def dynamics(
+ self, state: torch.Tensor, action: torch.Tensor
+ ) -> torch.Tensor:
+ """
+ Update robot state based on differential drive dynamics.
+ Args:
+ state (torch.Tensor): state batch tensor, shape (batch_size, 3) [x, y, theta, v]
+ action (torch.Tensor): control batch tensor, shape (batch_size, 2) [accel, steer]
+ delta_t (float): time step interval [s]
+ Returns:
+ torch.Tensor: shape (batch_size, 3) [x, y, theta]
+ """
+
+ # Perform calculations as before
+ x = state[:, 0].view(-1, 1)
+ y = state[:, 1].view(-1, 1)
+ theta = state[:, 2].view(-1, 1)
+ v = state[:, 3].view(-1, 1)
+ accel = torch.clamp(action[:, 0].view(-1, 1), self.u_min[0], self.u_max[0])
+ steer = torch.clamp(action[:, 1].view(-1, 1), self.u_min[1], self.u_max[1])
+ theta = angle_normalize(theta)
+
+ dx = v * torch.cos(theta)
+ dy = v * torch.sin(theta)
+ dv = accel
+ dtheta = v * torch.tan(steer) / self.vehicle_L
+
+ new_x = x + dx * self.delta_t
+ new_y = y + dy * self.delta_t
+ new_theta = angle_normalize(theta + dtheta * self.delta_t)
+ new_v = v + dv * self.delta_t
+
+ # Clamp velocity
+ new_v = torch.clamp(new_v, -self.V_MAX, self.V_MAX)
+
+ result = torch.cat([new_x, new_y, new_theta, new_v], dim=1)
+
+ return result
+
+
+ def cost_function(self, state: torch.Tensor, action: torch.Tensor, info: dict) -> torch.Tensor:
+ """
+ Calculate cost function
+ Args:
+ state (torch.Tensor): state batch tensor, shape (batch_size, 4) [x, y, theta, v]
+ action (torch.Tensor): control batch tensor, shape (batch_size, 2) [accel, steer]
+ Returns:
+ torch.Tensor: shape (batch_size,)
+ """
+ # info
+ prev_action = info["prev_action"]
+ t = info["t"] # horizon number
+
+ # path cost
+ # contouring and lag error of path
+ ec = torch.sin(self.reference_path[t, 2]) * (state[:, 0] - self.reference_path[t, 0]) \
+ -torch.cos(self.reference_path[t, 2]) * (state[:, 1] - self.reference_path[t, 1])
+ el = -torch.cos(self.reference_path[t, 2]) * (state[:, 0] - self.reference_path[t, 0]) \
+ -torch.sin(self.reference_path[t, 2]) * (state[:, 1] - self.reference_path[t, 1])
+
+ path_cost = self.Qc * ec.pow(2) + self.Ql * el.pow(2)
+
+ # velocity cost
+ v = state[:, 3]
+ v_target = self.reference_path[t, 3]
+ velocity_cost = self.Qv * (v - v_target).pow(2)
+
+ # compute obstacle cost from cost map
+ pos_batch = state[:, :2].unsqueeze(1) # (batch_size, 1, 2)
+ obstacle_cost = self.cost_map.compute_cost(pos_batch).squeeze(1) # (batch_size,)
+ obstacle_cost = self.Qo * obstacle_cost
+
+ # input cost
+ input_cost = self.Qin * action.pow(2).sum(dim=1)
+ input_cost += self.Qdin * (action - prev_action).pow(2).sum(dim=1)
+
+ cost = path_cost + velocity_cost + obstacle_cost + input_cost
+
+ return cost
+
+ def calc_ref_trajectory(self, state: torch.Tensor, path: torch.Tensor,
+ cind: int, horizon: int, DL=0.1, lookahead_distance=1.0, reference_path_interval=0.5
+ ) -> Tuple[torch.Tensor, int]:
+ """
+ Calculate the reference trajectory for the vehicle.
+
+ Args:
+ state (torch.Tensor): current state of the vehicle, shape (4,) [x, y, yaw, v]
+ path (torch.Tensor): reference path, shape (N, 4) [x, y, yaw, target_v]
+ cind (int): current index of the vehicle on the path
+ horizon (int): prediction horizon
+ DL (float): resolution of the path
+ lookahead_distance (float): distance to look ahead
+ reference_path_interval (float): interval of the reference path
+
+ Returns:
+ Tuple[torch.Tensor, int]: reference trajectory tensor, shape (horizon + 1, 4) [x, y, yaw, target_v], index of the vehicle on the path
+ """
+
+ def resample_path(path, DL):
+ # Calculate the number of segments needed for each pair of points
+ distances = torch.norm(path[1:, :2] - path[:-1, :2], dim=1)
+ num_points = torch.ceil(distances / DL).to(torch.int64)
+
+ # Create a tensor to store the new resampled path points
+ total_points = num_points.sum() + 1 # Include the first point
+ new_path = torch.zeros((total_points, path.shape[1]), dtype=path.dtype, device=path.device)
+
+ # Initialize the first point
+ new_path[0] = path[0]
+
+ # Generate all new points at once
+ start_idx = 1
+ for i in range(len(path) - 1):
+ segment_length = num_points[i].item()
+ if segment_length == 0:
+ continue
+
+ t = torch.linspace(0, 1, segment_length + 1, device=path.device)[1:] # Skip the first point to avoid duplication
+ interpolated_points = (1 - t).unsqueeze(1) * path[i] + t.unsqueeze(1) * path[i + 1]
+ new_path[start_idx:start_idx + segment_length] = interpolated_points
+ start_idx += segment_length
+
+ return new_path
+
+ # Resample the path with the specified DL
+ path = resample_path(path, DL)
+ ncourse = len(path)
+ xref = torch.zeros((horizon + 1, state.shape[0]), dtype=state.dtype, device=state.device)
+
+ # Calculate the nearest index to the vehicle
+ ind = torch.argmin(torch.norm(path[:, :2] - state[:2], dim=1)).item()
+ # Ensure the index is not less than the current index
+ ind = max(cind, ind)
+
+ # Generate the rest of the reference trajectory
+ travel = lookahead_distance
+
+ for i in range(horizon + 1):
+ travel += reference_path_interval
+ dind = int(round(travel / DL))
+
+ if (ind + dind) < ncourse:
+ xref[i] = path[ind + dind]
+ else:
+ xref[i] = path[-1]
+
+ return xref, ind
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py
new file mode 100644
index 00000000..a0153179
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/mppi_controller/mppi_controller_node.py
@@ -0,0 +1,250 @@
+import rclpy
+from rclpy.node import Node
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Pose, Point, Quaternion
+from autoware_auto_planning_msgs.msg import Trajectory, TrajectoryPoint
+from autoware_auto_control_msgs.msg import AckermannControlCommand
+from nav_msgs.msg import OccupancyGrid
+from rclpy.qos import QoSProfile
+import math
+import torch
+import numpy as np
+import tf_transformations
+from mppi_controller.mppi_controller import mppi_controller
+from mppi_controller.cost_map_tensor import CostMapTensor
+
+class MppiControllerNode(Node):
+
+ def __init__(self):
+ super().__init__('mppi_controller_node')
+
+ # parameters
+ # declare
+ self.declare_parameter('horizon', 25)
+ self.declare_parameter('num_samples', 4000)
+ self.declare_parameter('u_min', [-2.0, -0.25])
+ self.declare_parameter('u_max', [2.0, 0.25])
+ self.declare_parameter('sigmas', [0.5, 0.1])
+ self.declare_parameter('lambda', 1.0)
+ self.declare_parameter('auto_lambda', False)
+ self.declare_parameter('DL', 0.1)
+ self.declare_parameter('lookahead_distance', 3.0)
+ self.declare_parameter('reference_path_interval', 0.85)
+ self.declare_parameter('Qc', 2.0)
+ self.declare_parameter('Ql', 3.0)
+ self.declare_parameter('Qv', 2.0)
+ self.declare_parameter('Qo', 10000.0)
+ self.declare_parameter('Qin', 0.01)
+ self.declare_parameter('Qdin', 0.5)
+ self.declare_parameter('delta_t', 0.1)
+ self.declare_parameter('vehicle_L', 1.0)
+ self.declare_parameter('V_MAX', 8.0)
+ # get
+ config = {
+ "horizon": self.get_parameter('horizon').get_parameter_value().integer_value,
+ "num_samples": self.get_parameter('num_samples').get_parameter_value().integer_value,
+ "u_min": self.get_parameter('u_min').get_parameter_value().double_array_value,
+ "u_max": self.get_parameter('u_max').get_parameter_value().double_array_value,
+ "sigmas": self.get_parameter('sigmas').get_parameter_value().double_array_value,
+ "lambda": self.get_parameter('lambda').get_parameter_value().double_value,
+ "auto_lambda": self.get_parameter('auto_lambda').get_parameter_value().bool_value,
+ "DL": self.get_parameter('DL').get_parameter_value().double_value,
+ "lookahead_distance": self.get_parameter('lookahead_distance').get_parameter_value().double_value,
+ "reference_path_interval": self.get_parameter('reference_path_interval').get_parameter_value().double_value,
+ "Qc": self.get_parameter('Qc').get_parameter_value().double_value,
+ "Ql": self.get_parameter('Ql').get_parameter_value().double_value,
+ "Qv": self.get_parameter('Qv').get_parameter_value().double_value,
+ "Qo": self.get_parameter('Qo').get_parameter_value().double_value,
+ "Qin": self.get_parameter('Qin').get_parameter_value().double_value,
+ "Qdin": self.get_parameter('Qdin').get_parameter_value().double_value,
+ "delta_t": self.get_parameter('delta_t').get_parameter_value().double_value,
+ "vehicle_L": self.get_parameter('vehicle_L').get_parameter_value().double_value,
+ "V_MAX": self.get_parameter('V_MAX').get_parameter_value().double_value,
+ }
+
+ self.get_logger().info(f'config: {config}')
+
+ # publisher
+ # control command
+ self.pub_cmd = self.create_publisher(AckermannControlCommand, 'output/control_cmd', 1)
+ # planned path
+ self.pub_planned_path = self.create_publisher(Trajectory, 'output/planned_path', 1)
+ # debug path
+ self.pub_debug_path = self.create_publisher(Trajectory, 'debug/path', 1)
+
+ # subscriber
+ # state
+ self.sub_kinematics = self.create_subscription(
+ Odometry,
+ 'input/kinematics',
+ self.kinematics_callback,
+ 1
+ )
+ # reference trajectory
+ self.sub_trajectory = self.create_subscription(
+ Trajectory,
+ 'input/reference_trajectory',
+ self.trajectory_callback,
+ 1
+ )
+ # costmap
+ self.sub_costmap = self.create_subscription(
+ OccupancyGrid,
+ 'input/costmap',
+ self.costmap_callback,
+ 1
+ )
+
+ # device and dtype
+ self.device = torch.device("cuda") if torch.cuda.is_available() else torch.device("cpu")
+ self.dtype = torch.float32
+
+ # mppi controller
+ self.controller = mppi_controller(config=config, debug=True, device=self.device, dtype=self.dtype)
+
+ self.odometry: Odometry = None
+ self.trajectory: Trajectory = None
+ self.costmap: OccupancyGrid = None
+
+ self.timer = self.create_timer(0.03, self.on_timer)
+
+ def kinematics_callback(self, msg : Odometry):
+ self.odometry = msg
+
+ def trajectory_callback(self, msg : Trajectory):
+ self.trajectory = msg
+
+ def costmap_callback(self, msg : OccupancyGrid):
+ self.costmap = msg
+
+ def zero_ackermann_control_command(self):
+ cmd = AckermannControlCommand()
+ now = self.get_clock().now().to_msg()
+ cmd.stamp = now
+ cmd.longitudinal.stamp = now
+ cmd.longitudinal.speed = 0.0
+ cmd.longitudinal.acceleration = 0.0
+ cmd.lateral.stamp = now
+ cmd.lateral.steering_tire_angle = 0.0
+ return cmd
+
+ def on_timer(self):
+ if not self.subscribe_message_available():
+ return
+
+ cmd = self.zero_ackermann_control_command()
+
+ # convert tensor
+ # state
+ state_tensor = torch.tensor([
+ self.odometry.pose.pose.position.x,
+ self.odometry.pose.pose.position.y,
+ tf_transformations.euler_from_quaternion([
+ self.odometry.pose.pose.orientation.x,
+ self.odometry.pose.pose.orientation.y,
+ self.odometry.pose.pose.orientation.z,
+ self.odometry.pose.pose.orientation.w
+ ])[2],
+ self.odometry.twist.twist.linear.x
+ ], dtype=self.dtype, device=self.device)
+ # reference path
+ reference_path_tensor = torch.tensor([
+ [point.pose.position.x,
+ point.pose.position.y,
+ tf_transformations.euler_from_quaternion([
+ point.pose.orientation.x,
+ point.pose.orientation.y,
+ point.pose.orientation.z,
+ point.pose.orientation.w
+ ])[2],
+ point.longitudinal_velocity_mps] for point in self.trajectory.points
+ ], dtype=self.dtype, device=self.device)
+ # convert OccupancyGrid to tensor
+ costmap_tensor: CostMapTensor = CostMapTensor(
+ cost_map=torch.tensor(self.costmap.data, dtype=self.dtype, device=self.device).reshape(self.costmap.info.height, self.costmap.info.width),
+ cell_size=self.costmap.info.resolution,
+ origin=(self.costmap.info.origin.position.x,
+ self.costmap.info.origin.position.y),
+ device=self.device,
+ dtype=self.dtype
+ )
+ # set cost map
+ self.controller.set_cost_map(costmap_tensor)
+
+ # update controller
+ action_seq, state_seq = self.controller.update(state_tensor, reference_path_tensor)
+ # get top samples
+ top_samples = self.controller.get_top_samples(num_samples=300)
+
+ # convert numpy
+ state = state_seq.cpu().numpy()[0]
+ action = action_seq.cpu().numpy()[0]
+
+ # publish control command
+ cmd.longitudinal.speed = float(state[0, 3])
+ cmd.longitudinal.acceleration = float(action[0])
+ cmd.lateral.steering_tire_angle = float(action[1])
+ self.pub_cmd.publish(cmd)
+
+ # publish planned path
+ planned_path = Trajectory()
+ planned_path.header.stamp = self.get_clock().now().to_msg()
+ planned_path.header.frame_id = 'map'
+ planned_path.points = [
+ TrajectoryPoint(
+ pose=Pose(
+ position=Point(x=float(point[0]), y=float(point[1])),
+ orientation=Quaternion(
+ x=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[0],
+ y=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[1],
+ z=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[2],
+ w=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[3]
+ )
+ ),
+ longitudinal_velocity_mps=float(point[3])
+ )
+ for point in state]
+
+ self.pub_planned_path.publish(planned_path)
+
+ # publish debug path
+ debug_path = Trajectory()
+ debug_path.header.stamp = self.get_clock().now().to_msg()
+ debug_path.header.frame_id = 'map'
+ debug_path.points = [
+ TrajectoryPoint(
+ pose=Pose(
+ position=Point(x=float(point[0]), y=float(point[1])),
+ orientation=Quaternion(
+ x=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[0],
+ y=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[1],
+ z=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[2],
+ w=tf_transformations.quaternion_from_euler(0.0, 0.0, float(point[2]))[3]
+ )
+ ),
+ longitudinal_velocity_mps=float(point[3])
+ )
+ for point in self.controller.reference_path.cpu().numpy()]
+ self.pub_debug_path.publish(debug_path)
+
+
+ def subscribe_message_available(self):
+ if not self.odometry:
+ self.get_logger().info('odometry is not available', throttle_duration_sec=1)
+ return False
+ if not self.trajectory:
+ self.get_logger().info('trajectory is not available', throttle_duration_sec=1)
+ return False
+ if not self.costmap:
+ self.get_logger().info('costmap is not available', throttle_duration_sec=1)
+ return False
+ return True
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = MppiControllerNode()
+ rclpy.spin(node)
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml
new file mode 100644
index 00000000..e35b459d
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/package.xml
@@ -0,0 +1,16 @@
+
+
+
+ mppi_controller
+ 0.0.0
+ TODO: Package description
+ eguchi
+ TODO: License declaration
+
+ rclpy
+ rclpy
+
+
+ ament_python
+
+
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/resource/mppi_controller
new file mode 100644
index 00000000..e69de29b
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg
new file mode 100644
index 00000000..7d2409e1
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/mppi_controller
+[install]
+install_scripts=$base/lib/mppi_controller
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py
new file mode 100644
index 00000000..37ccad01
--- /dev/null
+++ b/aichallenge/workspace/src/aichallenge_submit/mppi_controller/setup.py
@@ -0,0 +1,33 @@
+import subprocess
+import sys
+
+# dependency
+subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'setuptools==58.0.4', '--quiet'])
+subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'torch', '--index-url', 'https://download.pytorch.org/whl/cpu', '--quiet'])
+subprocess.check_call([sys.executable, '-m', 'pip', 'install', 'transforms3d', '--quiet'])
+
+from setuptools import setup, find_packages
+package_name = 'mppi_controller'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(),
+ data_files=[
+ ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ('share/' + package_name + '/config', ['config/mppi_controller.param.yaml']),
+ ],
+ install_requires=['setuptools', 'scipy'],
+ zip_safe=True,
+ maintainer='michikuni eguchi',
+ maintainer_email='egrt117@gmail.com',
+ description='MPPI controller',
+ license='License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'mppi_controller_node = mppi_controller.mppi_controller_node:main',
+ ],
+ },
+)
\ No newline at end of file
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz
index 1539328f..9e75c143 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/config/autoware.rviz
@@ -3,9 +3,15 @@ Panels:
Help Height: 0
Name: Displays
Property Tree Widget:
- Expanded: ~
+ Expanded:
+ - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1
+ - /Planning1/ScenarioPlanning1/ScenarioTrajectory1
+ - /Planning1/ScenarioPlanning1/ScenarioTrajectory1/View Path1
+ - /Planning1/Costmap1/Map1
+ - /Planning1/MPPI1
+ - /Planning1/MPPI1/Trajectory1
Splitter Ratio: 0.557669460773468
- Tree Height: 397
+ Tree Height: 158
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -13,7 +19,8 @@ Panels:
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
- Expanded: ~
+ Expanded:
+ - /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel
@@ -39,7 +46,8 @@ Visualization Manager:
Show Arrows: true
Show Axes: true
Show Names: true
- Tree: {}
+ Tree:
+ {}
Update Interval: 0
Value: false
- Alpha: 0.5
@@ -64,9 +72,12 @@ Visualization Manager:
Displays:
- Class: rviz_plugins/SteeringAngle
Enabled: true
+ Left: 85
+ Length: 171
Name: SteeringAngle
Scale: 17
Text Color: 25; 255; 240
+ Top: 85
Topic:
Depth: 5
Durability Policy: Volatile
@@ -74,12 +85,15 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /vehicle/status/steering_status
Value: true
+ Value Scale: 0.14999249577522278
Value height offset: 0
- Class: rviz_plugins/ConsoleMeter
Enabled: true
+ Left: 341
+ Length: 171
Name: ConsoleMeter
- Scale: 3
Text Color: 25; 255; 240
+ Top: 85
Topic:
Depth: 5
Durability Policy: Volatile
@@ -87,8 +101,9 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /vehicle/status/velocity_status
Value: true
+ Value Scale: 0.14999249577522278
Value height offset: 0
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Class: rviz_plugins/VelocityHistory
Color Border Vel Max: 3
Constant Color:
@@ -157,8 +172,8 @@ Visualization Manager:
Max Alpha: 0.5
Max Range: 100
Max Wave Alpha: 0.5
- Min Alpha: 0.01
- Min Wave Alpha: 0.01
+ Min Alpha: 0.009999999776482582
+ Min Wave Alpha: 0.009999999776482582
Name: PolarGridDisplay
Reference Frame: base_link
Value: true
@@ -166,18 +181,20 @@ Visualization Manager:
Wave Velocity: 40
- Class: rviz_plugins/MaxVelocity
Enabled: true
+ Left: 397
+ Length: 64
Name: MaxVelocity
Text Color: 255; 255; 255
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /planning/scenario_planning/current_max_velocity
+ Top: 187
+ Topic: /planning/scenario_planning/current_max_velocity
Value: true
+ Value Scale: 0.25
- Class: rviz_plugins/TurnSignal
Enabled: true
+ Height: 171
+ Left: 131
Name: TurnSignal
+ Top: 233
Topic:
Depth: 5
Durability Policy: Volatile
@@ -185,6 +202,7 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /vehicle/status/turn_indicators_status
Value: true
+ Width: 341
Enabled: true
Name: Vehicle
Enabled: true
@@ -213,11 +231,12 @@ Visualization Manager:
Position Transformer: XYZ
Selectable: false
Size (Pixels): 1
- Size (m): 0.02
+ Size (m): 0.019999999552965164
Style: Points
Topic:
Depth: 5
Durability Policy: Transient Local
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /map/pointcloud_map
@@ -248,7 +267,7 @@ Visualization Manager:
Displays:
- Class: rviz_common/Group
Displays:
- - Alpha: 0.4
+ - Alpha: 0.4000000059604645
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 5
@@ -270,18 +289,19 @@ Visualization Manager:
Position Transformer: XYZ
Selectable: false
Size (Pixels): 1
- Size (m): 0.02
+ Size (m): 0.019999999552965164
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /sensing/lidar/concatenated/pointcloud
Use Fixed Frame: false
Use rainbow: true
Value: true
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Class: rviz_default_plugins/Polygon
Color: 25; 255; 0
Enabled: false
@@ -289,6 +309,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /sensing/lidar/crop_box_filter/crop_box_polygon
@@ -327,6 +348,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /sensing/gnss/pose_with_covariance
@@ -339,7 +361,7 @@ Visualization Manager:
Displays:
- Class: rviz_common/Group
Displays:
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz_default_plugins/PoseWithCovariance
@@ -369,11 +391,12 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /localization/pose_estimator/initial_pose_with_covariance
- Value: true
- - Alpha: 0.999
+ Value: false
+ - Alpha: 0.9990000128746033
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz_default_plugins/PoseWithCovariance
@@ -403,27 +426,29 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /localization/pose_estimator/pose_with_covariance
- Value: true
+ Value: false
- Buffer Size: 200
Class: rviz_plugins::PoseHistory
Enabled: false
Line:
+ Alpha: 0.9990000128746033
Color: 170; 255; 127
Value: true
Width: 0.10000000149011612
- Alpha: 0.999
Name: PoseHistory
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /localization/pose_estimator/pose
- Value: true
- - Alpha: 0.999
+ Value: false
+ - Alpha: 0.9990000128746033
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
@@ -450,13 +475,14 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /localization/util/downsample/pointcloud
Use Fixed Frame: true
Use rainbow: true
Value: false
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
@@ -483,6 +509,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /localization/pose_estimator/points_aligned
@@ -492,7 +519,8 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MonteCarloInitialPose
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -508,14 +536,15 @@ Visualization Manager:
Class: rviz_plugins::PoseHistory
Enabled: true
Line:
+ Alpha: 0.9990000128746033
Color: 0; 255; 255
Value: true
Width: 0.10000000149011612
- Alpha: 0.999
Name: PoseHistory
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /localization/pose_twist_fusion_filter/pose
@@ -528,7 +557,7 @@ Visualization Manager:
Displays:
- Class: rviz_common/Group
Displays:
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 15
@@ -550,11 +579,12 @@ Visualization Manager:
Position Transformer: XYZ
Selectable: false
Size (Pixels): 3
- Size (m): 0.02
+ Size (m): 0.019999999552965164
Style: Points
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /perception/obstacle_segmentation/pointcloud
@@ -568,16 +598,16 @@ Visualization Manager:
- Class: rviz_common/Group
Displays:
- BUS:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
CAR:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
CYCLIST:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 119; 11; 32
Class: autoware_auto_perception_rviz_plugin/DetectedObjects
- Display 3d polygon: true
+ Display Acceleration: true
Display Label: true
Display PoseWithCovariance: true
Display Predicted Path Confidence: true
@@ -585,21 +615,23 @@ Visualization Manager:
Display Twist: true
Display UUID: true
Display Velocity: true
- Line Width: 0.03
Enabled: true
+ Line Width: 0.029999999329447746
MOTORCYCLE:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 119; 11; 32
Name: DetectedObjects
- Namespaces: {}
+ Namespaces:
+ {}
PEDESTRIAN:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 255; 192; 203
+ Polygon Type: 3d
TRAILER:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
TRUCK:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
Topic:
Depth: 5
@@ -608,24 +640,25 @@ Visualization Manager:
Reliability Policy: Best Effort
Value: /perception/object_recognition/detection/objects
UNKNOWN:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 255; 255; 255
Value: true
+ Visualization Type: Normal
Enabled: false
Name: Detection
- Class: rviz_common/Group
Displays:
- BUS:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
CAR:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
CYCLIST:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 119; 11; 32
Class: autoware_auto_perception_rviz_plugin/TrackedObjects
- Display 3d polygon: true
+ Display Acceleration: true
Display Label: true
Display PoseWithCovariance: true
Display Predicted Path Confidence: true
@@ -633,21 +666,23 @@ Visualization Manager:
Display Twist: true
Display UUID: true
Display Velocity: true
- Line Width: 0.03
Enabled: true
+ Line Width: 0.029999999329447746
MOTORCYCLE:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 119; 11; 32
Name: TrackedObjects
- Namespaces: {}
+ Namespaces:
+ {}
PEDESTRIAN:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 255; 192; 203
+ Polygon Type: 3d
TRAILER:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
TRUCK:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
Topic:
Depth: 5
@@ -656,24 +691,25 @@ Visualization Manager:
Reliability Policy: Best Effort
Value: /perception/object_recognition/tracking/objects
UNKNOWN:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 255; 255; 255
Value: true
+ Visualization Type: Normal
Enabled: false
Name: Tracking
- Class: rviz_common/Group
Displays:
- BUS:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
CAR:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
CYCLIST:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 119; 11; 32
Class: autoware_auto_perception_rviz_plugin/PredictedObjects
- Display 3d polygon: true
+ Display Acceleration: true
Display Label: true
Display PoseWithCovariance: false
Display Predicted Path Confidence: true
@@ -681,21 +717,28 @@ Visualization Manager:
Display Twist: true
Display UUID: true
Display Velocity: true
- Line Width: 0.03
Enabled: true
+ Line Width: 0.029999999329447746
MOTORCYCLE:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 119; 11; 32
Name: PredictedObjects
- Namespaces: {}
+ Namespaces:
+ acceleration: true
+ label: true
+ shape: true
+ twist: true
+ uuid: true
+ velocity: true
PEDESTRIAN:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 255; 192; 203
+ Polygon Type: 3d
TRAILER:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
TRUCK:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 30; 144; 255
Topic:
Depth: 5
@@ -704,20 +747,22 @@ Visualization Manager:
Reliability Policy: Best Effort
Value: /perception/object_recognition/objects
UNKNOWN:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 255; 255; 255
Value: true
+ Visualization Type: Normal
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Maneuver
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Best Effort
Value: /perception/object_recognition/prediction/maneuver
- Value: true
+ Value: false
Enabled: true
Name: Prediction
Enabled: true
@@ -741,7 +786,8 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: MapBasedDetectionResult
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -798,7 +844,7 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /planning/mission_planning/route_marker
Value: true
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Axes Length: 1
Axes Radius: 0.30000001192092896
Class: rviz_default_plugins/Pose
@@ -813,6 +859,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/mission_planning/echo_back_goal_pose
@@ -822,24 +869,42 @@ Visualization Manager:
- Class: rviz_common/Group
Displays:
- Class: rviz_plugins/Trajectory
- Color Border Vel Max: 3
+ Color Border Vel Max: 8
Enabled: true
Name: ScenarioTrajectory
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/scenario_planning/trajectory
Value: true
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 0; 0; 0
Constant Color: false
Value: true
- Width: 2
+ Width: 0.5
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 0; 0; 0
Constant Color: false
Scale: 0.30000001192092896
@@ -855,16 +920,39 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/scenario_planning/lane_driving/behavior_planning/path
Value: true
+ View Drivable Area:
+ Alpha: 0.9990000128746033
+ Color: 0; 148; 205
+ Value: true
+ Width: 0.30000001192092896
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 0.4000000059604645
Color: 0; 0; 0
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 0.4000000059604645
Color: 0; 0; 0
@@ -878,16 +966,39 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/path_candidate/lane_change
Value: true
+ View Drivable Area:
+ Alpha: 0.9990000128746033
+ Color: 0; 148; 205
+ Value: true
+ Width: 0.30000001192092896
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 0.30000001192092896
Color: 115; 210; 22
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 0.30000001192092896
Color: 0; 0; 0
@@ -901,16 +1012,39 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/path_candidate/ext_request_lane_change_right
Value: true
+ View Drivable Area:
+ Alpha: 0.9990000128746033
+ Color: 0; 148; 205
+ Value: true
+ Width: 0.30000001192092896
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 0.30000001192092896
Color: 115; 210; 22
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 0.30000001192092896
Color: 0; 0; 0
@@ -924,16 +1058,39 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/path_candidate/ext_request_lane_change_left
Value: true
+ View Drivable Area:
+ Alpha: 0.9990000128746033
+ Color: 0; 148; 205
+ Value: true
+ Width: 0.30000001192092896
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 0.30000001192092896
Color: 115; 210; 22
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 0.30000001192092896
Color: 0; 0; 0
@@ -947,16 +1104,39 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/path_candidate/avoidance
Value: true
+ View Drivable Area:
+ Alpha: 0.9990000128746033
+ Color: 0; 148; 205
+ Value: true
+ Width: 0.30000001192092896
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 0.30000001192092896
Color: 115; 210; 22
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 0.30000001192092896
Color: 0; 0; 0
@@ -970,16 +1150,39 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/path_candidate/pull_out
Value: true
+ View Drivable Area:
+ Alpha: 0.9990000128746033
+ Color: 0; 148; 205
+ Value: true
+ Width: 0.30000001192092896
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 0.30000001192092896
Color: 115; 210; 22
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 0.30000001192092896
Color: 0; 0; 0
@@ -993,16 +1196,39 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/path_candidate/pull_over
Value: true
+ View Drivable Area:
+ Alpha: 0.9990000128746033
+ Color: 0; 148; 205
+ Value: true
+ Width: 0.30000001192092896
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 0.30000001192092896
Color: 115; 210; 22
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 0.30000001192092896
Color: 0; 0; 0
@@ -1018,7 +1244,6 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
- Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound
@@ -1414,18 +1639,36 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/scenario_planning/lane_driving/trajectory
Value: false
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 0; 0; 0
Constant Color: false
Value: true
Width: 2
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
- Alpha: 0.999
+ Alpha: 0.9990000128746033
Color: 0; 0; 0
Constant Color: false
Scale: 0.30000001192092896
@@ -1608,6 +1851,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid
@@ -1619,7 +1863,7 @@ Visualization Manager:
Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates
Use Timestamp: false
Value: false
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Arrow Length: 0.30000001192092896
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
@@ -1635,11 +1879,12 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array
Value: true
- - Alpha: 0.999
+ - Alpha: 0.9990000128746033
Arrow Length: 0.5
Axes Length: 0.30000001192092896
Axes Radius: 0.009999999776482582
@@ -1655,6 +1900,7 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array
@@ -1684,7 +1930,8 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: true
Name: PlanningErrorMarker
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -1694,6 +1941,76 @@ Visualization Manager:
Value: true
Enabled: false
Name: Diagnostic
+ - Class: rviz_common/Group
+ Displays:
+ - Alpha: 0.699999988079071
+ Class: rviz_default_plugins/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ Filter size: 10
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/costmap/costmap_generator/costmap
+ Update Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/costmap/costmap_generator/costmap_updates
+ Use Timestamp: false
+ Value: true
+ Enabled: true
+ Name: Costmap
+ - Class: rviz_common/Group
+ Displays:
+ - Class: rviz_plugins/Trajectory
+ Color Border Vel Max: 8
+ Enabled: true
+ Name: Trajectory
+ Topic:
+ Depth: 1
+ Durability Policy: Volatile
+ Filter size: 11
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /planning/output/mppi_planned_path
+ Value: true
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
+ View Path:
+ Alpha: 1
+ Color: 0; 0; 0
+ Constant Color: false
+ Value: true
+ Width: 0.5
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
+ View Velocity:
+ Alpha: 1
+ Color: 0; 0; 0
+ Constant Color: false
+ Scale: 0.30000001192092896
+ Value: true
+ Enabled: true
+ Name: MPPI
Enabled: true
Name: Planning
- Class: rviz_common/Group
@@ -1705,16 +2022,34 @@ Visualization Manager:
Topic:
Depth: 5
Durability Policy: Volatile
+ Filter size: 10
History Policy: Keep Last
Reliability Policy: Reliable
Value: /control/trajectory_follower/lateral/predicted_trajectory
Value: true
+ View Footprint:
+ Alpha: 1
+ Color: 230; 230; 50
+ Offset from BaseLink: 0
+ Rear Overhang: 1.0299999713897705
+ Value: false
+ Vehicle Length: 4.769999980926514
+ Vehicle Width: 1.8300000429153442
View Path:
Alpha: 1
Color: 255; 255; 255
Constant Color: true
Value: true
Width: 0.05000000074505806
+ View Point:
+ Alpha: 1
+ Color: 0; 60; 255
+ Offset: 0
+ Radius: 0.10000000149011612
+ Value: false
+ View Text Velocity:
+ Scale: 0.30000001192092896
+ Value: false
View Velocity:
Alpha: 1
Color: 0; 0; 0
@@ -1724,7 +2059,8 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Debug/MPC
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -1735,7 +2071,8 @@ Visualization Manager:
- Class: rviz_default_plugins/MarkerArray
Enabled: false
Name: Debug/PurePursuit
- Namespaces: {}
+ Namespaces:
+ {}
Topic:
Depth: 5
Durability Policy: Volatile
@@ -1797,15 +2134,15 @@ Visualization Manager:
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
- Theta std deviation: 0.2617993950843811
+ Covariance x: 0.25
+ Covariance y: 0.25
+ Covariance yaw: 0.06853891909122467
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- X std deviation: 0.5
- Y std deviation: 0.5
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
@@ -1813,26 +2150,47 @@ Visualization Manager:
History Policy: Keep Last
Reliability Policy: Reliable
Value: /planning/mission_planning/goal
- - Class: rviz_plugins/PedestrianInitialPoseTool
+ - Acceleration: 0
+ Class: rviz_plugins/PedestrianInitialPoseTool
+ Interactive: false
+ Max velocity: 33.29999923706055
+ Min velocity: -33.29999923706055
Pose Topic: /simulation/dummy_perception_publisher/object_info
+ Target Frame:
Theta std deviation: 0.0872664600610733
Velocity: 0
X std deviation: 0.029999999329447746
Y std deviation: 0.029999999329447746
Z position: 1
Z std deviation: 0.029999999329447746
- - Class: rviz_plugins/CarInitialPoseTool
+ - Acceleration: 0
+ Class: rviz_plugins/CarInitialPoseTool
+ H vehicle height: 2
+ Interactive: false
+ L vehicle length: 4
+ Max velocity: 33.29999923706055
+ Min velocity: -33.29999923706055
Pose Topic: /simulation/dummy_perception_publisher/object_info
+ Target Frame:
Theta std deviation: 0.0872664600610733
Velocity: 3
+ W vehicle width: 1.7999999523162842
X std deviation: 0.029999999329447746
Y std deviation: 0.029999999329447746
Z position: 0
Z std deviation: 0.029999999329447746
- - Class: rviz_plugins/BusInitialPoseTool
+ - Acceleration: 0
+ Class: rviz_plugins/BusInitialPoseTool
+ H vehicle height: 3.5
+ Interactive: false
+ L vehicle length: 10.5
+ Max velocity: 33.29999923706055
+ Min velocity: -33.29999923706055
Pose Topic: /simulation/dummy_perception_publisher/object_info
+ Target Frame:
Theta std deviation: 0.0872664600610733
Velocity: 0
+ W vehicle width: 2.5
X std deviation: 0.029999999329447746
Y std deviation: 0.029999999329447746
Z position: 0
@@ -1845,6 +2203,9 @@ Visualization Manager:
Z position: 0
- Class: rviz_plugins/DeleteAllObjectsTool
Pose Topic: /simulation/dummy_perception_publisher/object_info
+ Transformation:
+ Current:
+ Class: rviz_default_plugins/TF
Value: true
Views:
Current:
@@ -1858,11 +2219,11 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Scale: 10
+ Scale: 7.122405529022217
Target Frame: viewer
Value: TopDownOrtho (rviz_default_plugins)
- X: 0
- Y: 0
+ X: 2.5461606979370117
+ Y: -8.487200736999512
Saved:
- Class: rviz_default_plugins/ThirdPersonFollower
Distance: 18
@@ -1908,12 +2269,12 @@ Window Geometry:
collapsed: false
Displays:
collapsed: false
- Height: 1016
+ Height: 1043
Hide Left Dock: false
Hide Right Dock: false
InitialPoseButtonPanel:
collapsed: false
- QMainWindow State: 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
+ QMainWindow State: 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
RecognitionResultOnImage:
collapsed: false
Selection:
@@ -1923,5 +2284,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1920
- X: 70
- Y: 27
+ X: 0
+ Y: 32
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 eda86a3a..9969163f 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
@@ -9,7 +9,7 @@
-
+