Skip to content

Commit aadf3c2

Browse files
adivardiadivardi
committed
Revert "[clean] pub unconstrained optimal trajectory"
This reverts commit a3f4b4b.
1 parent af63f9d commit aadf3c2

File tree

5 files changed

+20
-57
lines changed

5 files changed

+20
-57
lines changed

‎nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -110,16 +110,14 @@ class MPPIController : public nav2_core::Controller
110110
void visualize(
111111
nav_msgs::msg::Path transformed_plan,
112112
const builtin_interfaces::msg::Time & cmd_stamp,
113-
const Eigen::ArrayXXf & optimal_trajectory,
114-
const Eigen::ArrayXXf & optimal_trajectory_unconstrained);
113+
const Eigen::ArrayXXf & optimal_trajectory);
115114

116115
std::string name_;
117116
nav2::LifecycleNode::WeakPtr parent_;
118117
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
119118
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
120119
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
121120
nav2::Publisher<nav2_msgs::msg::Trajectory>::SharedPtr opt_traj_pub_;
122-
nav2::Publisher<nav2_msgs::msg::Trajectory>::SharedPtr opt_traj_unconstrained_pub_;
123121

124122
std::unique_ptr<ParametersHandler> parameters_handler_;
125123
Optimizer optimizer_;

‎nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp‎

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -109,15 +109,14 @@ class Optimizer
109109
* @brief Get the optimal trajectory for a cycle for visualization
110110
* @return Optimal trajectory
111111
*/
112-
Eigen::ArrayXXf getOptimizedTrajectory(const models::ControlSequence& control_sequence) const;
112+
Eigen::ArrayXXf getOptimizedTrajectory();
113113

114114
/**
115115
* @brief Get the optimal control sequence for a cycle for visualization
116116
* @return Optimal control sequence
117117
*/
118118
const models::ControlSequence & getOptimalControlSequence();
119-
const models::ControlSequence & getOptimalControlSequenceUnconstrained();
120-
const Eigen::ArrayXXf & getOptimalTrajectoryUnconstrained();
119+
121120
/**
122121
* @brief Set the maximum speed based on the speed limits callback
123122
* @param speed_limit Limit of the speed for use
@@ -278,7 +277,6 @@ class Optimizer
278277

279278
models::State state_;
280279
models::ControlSequence control_sequence_;
281-
models::ControlSequence control_sequence_virtual_;
282280
std::array<mppi::models::Control, 4> control_history_;
283281
models::Trajectories generated_trajectories_;
284282
models::Path path_;
@@ -288,8 +286,6 @@ class Optimizer
288286
models::ControlSequence prev_control_sequence_;
289287
Eigen::Array3f initial_velocities_;
290288

291-
Eigen::ArrayXXf optimal_trajectory_unconstrained_;
292-
293289
CriticData critics_data_ = {
294290
state_, generated_trajectories_, path_, goal_,
295291
costs_, settings_.model_dt, false, nullptr, nullptr,

‎nav2_mppi_controller/src/controller.cpp‎

Lines changed: 2 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -52,9 +52,6 @@ void MPPIController::configure(
5252
"~/optimal_trajectory");
5353
}
5454

55-
opt_traj_unconstrained_pub_ = node->create_publisher<nav2_msgs::msg::Trajectory>(
56-
"~/optimal_trajectory_unconstrained");
57-
5855
RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
5956
}
6057

@@ -127,21 +124,8 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
127124
opt_traj_pub_->publish(std::move(trajectory_msg));
128125
}
129126

130-
// if (publish_optimal_trajectory_ && opt_traj_unconstrained_pub_->get_subscription_count() > 0) {
131-
// std_msgs::msg::Header trajectory_header;
132-
// trajectory_header.stamp = cmd.header.stamp;
133-
// trajectory_header.frame_id = costmap_ros_->getGlobalFrameID();
134-
135-
// auto trajectory_msg = utils::toTrajectoryMsg(
136-
// optimizer_.getOptimalTrajectoryUnconstrained(),
137-
// optimizer_.getOptimalControlSequenceUnconstrained(),
138-
// optimizer_.getSettings().model_dt,
139-
// trajectory_header);
140-
// opt_traj_unconstrained_pub_->publish(std::move(trajectory_msg));
141-
// }
142-
143127
if (visualize_) {
144-
visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory, optimizer_.getOptimalTrajectoryUnconstrained());
128+
visualize(std::move(transformed_plan), cmd.header.stamp, optimal_trajectory);
145129
}
146130

147131
return cmd;
@@ -150,12 +134,10 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
150134
void MPPIController::visualize(
151135
nav_msgs::msg::Path transformed_plan,
152136
const builtin_interfaces::msg::Time & cmd_stamp,
153-
const Eigen::ArrayXXf & optimal_trajectory,
154-
const Eigen::ArrayXXf & optimal_trajectory_unconstrained)
137+
const Eigen::ArrayXXf & optimal_trajectory)
155138
{
156139
trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
157140
trajectory_visualizer_.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
158-
trajectory_visualizer_.add(optimal_trajectory_unconstrained, "Optimal Trajectory Unconstrained", cmd_stamp);
159141
trajectory_visualizer_.visualize(std::move(transformed_plan));
160142
}
161143

‎nav2_mppi_controller/src/optimizer.cpp‎

Lines changed: 7 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -188,16 +188,14 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
188188
{
189189
prepare(robot_pose, robot_speed, plan, goal, goal_checker);
190190
Eigen::ArrayXXf optimal_trajectory;
191-
Eigen::ArrayXXf optimal_trajectory_unconstraint;
192191
bool trajectory_valid = true;
193192

194193
do {
195194
// std::cout << "\n\t control_sequence_ Before optimize:\n\t\t"
196195
// << control_sequence_.vx(Eigen::seq(0, 5)).transpose() << "\n\t"
197196
// << control_sequence_.wz(Eigen::seq(0, 5)).transpose() << "\n\t" << std::endl;
198197
optimize();
199-
optimal_trajectory = getOptimizedTrajectory(control_sequence_);
200-
optimal_trajectory_unconstrained_ = getOptimizedTrajectory(control_sequence_virtual_);
198+
optimal_trajectory = getOptimizedTrajectory();
201199
switch (trajectory_validator_->validateTrajectory(
202200
optimal_trajectory, control_sequence_, robot_pose, robot_speed, plan, goal))
203201
{
@@ -216,8 +214,8 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
216214
} while (fallback(critics_data_.fail_flag || !trajectory_valid));
217215

218216
// std::cout << "Control Sequence End:\n";
219-
// // std::cout << "vx: " << control_sequence_.vx.transpose() << "\n";
220-
// // std::cout << "wz: " << control_sequence_.wz.transpose() << "\n";
217+
// std::cout << "vx: " << control_sequence_.vx.transpose() << "\n";
218+
// std::cout << "wz: " << control_sequence_.wz.transpose() << "\n";
221219
// computeControlSequenceAccel(control_sequence_);
222220

223221
auto control = getControlFromSequenceAsTwist(plan.header.stamp);
@@ -594,18 +592,18 @@ void Optimizer::integrateStateVelocities(
594592
}
595593
}
596594

597-
Eigen::ArrayXXf Optimizer::getOptimizedTrajectory(const models::ControlSequence& control_sequence) const
595+
Eigen::ArrayXXf Optimizer::getOptimizedTrajectory()
598596
{
599597
const bool is_holo = isHolonomic();
600598
Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2);
601599
Eigen::Array<float, Eigen::Dynamic, 3> trajectories =
602600
Eigen::Array<float, Eigen::Dynamic, 3>(settings_.time_steps, 3);
603601

604-
sequence.col(0) = control_sequence.vx;
605-
sequence.col(1) = control_sequence.wz;
602+
sequence.col(0) = control_sequence_.vx;
603+
sequence.col(1) = control_sequence_.wz;
606604

607605
if (is_holo) {
608-
sequence.col(2) = control_sequence.vy;
606+
sequence.col(2) = control_sequence_.vy;
609607
}
610608

611609
integrateStateVelocities(trajectories, sequence);
@@ -617,17 +615,6 @@ const models::ControlSequence & Optimizer::getOptimalControlSequence()
617615
return control_sequence_;
618616
}
619617

620-
621-
const models::ControlSequence & Optimizer::getOptimalControlSequenceUnconstrained()
622-
{
623-
return control_sequence_virtual_;
624-
}
625-
626-
const Eigen::ArrayXXf & Optimizer::getOptimalTrajectoryUnconstrained()
627-
{
628-
return optimal_trajectory_unconstrained_;
629-
}
630-
631618
void Optimizer::updateControlSequence()
632619
{
633620
const bool is_holo = isHolonomic();

‎nav2_mppi_controller/test/optimizer_unit_tests.cpp‎

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -249,14 +249,14 @@ TEST(OptimizerTests, BasicInitializedFunctions)
249249
EXPECT_EQ(trajs.x.cols(), 50);
250250
EXPECT_TRUE(trajs.x.isApproxToConstant(0.0f));
251251

252-
// optimizer_tester.resetMotionModel();
253-
// optimizer_tester.testSetOmniModel();
254-
// auto traj = optimizer_tester.getOptimizedTrajectory();
255-
// EXPECT_EQ(traj(5, 0), 0.0); // x
256-
// EXPECT_EQ(traj(5, 1), 0.0); // y
257-
// EXPECT_EQ(traj(5, 2), 0.0); // yaw
258-
// EXPECT_EQ(traj.rows(), 50);
259-
// EXPECT_EQ(traj.cols(), 3);
252+
optimizer_tester.resetMotionModel();
253+
optimizer_tester.testSetOmniModel();
254+
auto traj = optimizer_tester.getOptimizedTrajectory();
255+
EXPECT_EQ(traj(5, 0), 0.0); // x
256+
EXPECT_EQ(traj(5, 1), 0.0); // y
257+
EXPECT_EQ(traj(5, 2), 0.0); // yaw
258+
EXPECT_EQ(traj.rows(), 50);
259+
EXPECT_EQ(traj.cols(), 3);
260260

261261
optimizer_tester.reset();
262262
optimizer_tester.shutdown();

0 commit comments

Comments
 (0)