Skip to content

Commit 1a2df95

Browse files
alyquantillionalyquantillion
committed
move publish_optimal_path to TrajectoryVisualizer + minor refactoring
Signed-off-by: Alyssa Agnissan <alyssa.agnissan@quantillion.io>
1 parent a2fd2e3 commit 1a2df95

File tree

4 files changed

+54
-52
lines changed

4 files changed

+54
-52
lines changed

‎nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp‎

Lines changed: 1 addition & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -106,13 +106,7 @@ class MPPIController : public nav2_core::Controller
106106
* @brief Visualize trajectories
107107
* @param transformed_plan Transformed input plan
108108
*/
109-
void visualize(nav_msgs::msg::Path transformed_plan);
110-
111-
/**
112-
* @brief Publish the optimal trajectory in the form of a path message
113-
* @param trajectory Optimal trajectory
114-
*/
115-
void publish_optimal_path(const xt::xtensor<float, 2> & optimal_traj);
109+
void visualize(nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp);
116110

117111
std::string name_;
118112
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
@@ -125,8 +119,6 @@ class MPPIController : public nav2_core::Controller
125119
PathHandler path_handler_;
126120
TrajectoryVisualizer trajectory_visualizer_;
127121

128-
rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>::SharedPtr optimal_path_pub_;
129-
130122
bool visualize_;
131123
};
132124

‎nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -87,11 +87,17 @@ class TrajectoryVisualizer
8787
*/
8888
void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
8989

90+
/**
91+
* @brief Publish the optimal trajectory in the form of a path message
92+
* @param trajectory Optimal trajectory
93+
*/
94+
void publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp);
95+
9096
/**
9197
* @brief Visualize the plan
9298
* @param plan Plan to visualize
9399
*/
94-
void visualize(const nav_msgs::msg::Path & plan);
100+
void visualize(const nav_msgs::msg::Path & plan,const builtin_interfaces::msg::Time & cmd_stamp);
95101

96102
/**
97103
* @brief Reset object
@@ -103,7 +109,9 @@ class TrajectoryVisualizer
103109
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>>
104110
trajectories_publisher_;
105111
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_path_pub_;
112+
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> optimal_path_pub_;
106113

114+
std::unique_ptr<xt::xtensor<float, 2>> optimal_traj_;
107115
std::unique_ptr<visualization_msgs::msg::MarkerArray> points_;
108116
int marker_id_ = 0;
109117

‎nav2_mppi_controller/src/controller.cpp‎

Lines changed: 3 additions & 41 deletions
Original file line numberDiff line numberDiff line change
@@ -45,8 +45,6 @@ void MPPIController::configure(
4545
parent_, name_,
4646
costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
4747

48-
optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("local_plan", 1);
49-
5048
RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
5149
}
5250

@@ -62,14 +60,12 @@ void MPPIController::activate()
6260
{
6361
trajectory_visualizer_.on_activate();
6462
parameters_handler_->start();
65-
optimal_path_pub_->on_activate();
6663
RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
6764
}
6865

6966
void MPPIController::deactivate()
7067
{
7168
trajectory_visualizer_.on_deactivate();
72-
optimal_path_pub_->on_deactivate();
7369
RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
7470
}
7571

@@ -102,22 +98,18 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
10298
RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
10399
#endif
104100

105-
if (optimal_path_pub_->get_subscription_count() > 0) {
106-
publish_optimal_path(optimizer_.getOptimizedTrajectory());
107-
}
108-
109101
if (visualize_) {
110-
visualize(std::move(transformed_plan));
102+
visualize(std::move(transformed_plan), cmd.header.stamp);
111103
}
112104

113105
return cmd;
114106
}
115107

116-
void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
108+
void MPPIController::visualize(nav_msgs::msg::Path transformed_plan, const builtin_interfaces::msg::Time & cmd_stamp)
117109
{
118110
trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
119111
trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
120-
trajectory_visualizer_.visualize(std::move(transformed_plan));
112+
trajectory_visualizer_.visualize(std::move(transformed_plan), cmd_stamp);
121113
}
122114

123115
void MPPIController::setPlan(const nav_msgs::msg::Path & path)
@@ -130,36 +122,6 @@ void MPPIController::setSpeedLimit(const double & speed_limit, const bool & perc
130122
optimizer_.setSpeedLimit(speed_limit, percentage);
131123
}
132124

133-
void MPPIController::publish_optimal_path(const xt::xtensor<float, 2> & optimal_traj)
134-
{
135-
auto & size = optimal_traj.shape()[0];
136-
if (!size) {
137-
return;
138-
}
139-
140-
nav_msgs::msg::Path optimal_path;
141-
optimal_path.header.stamp = clock_->now();
142-
optimal_path.header.frame_id = (costmap_ros_->getGlobalFrameID()).c_str();
143-
144-
for (size_t i = 0; i < size; i++) {
145-
// create new pose for the path
146-
geometry_msgs::msg::PoseStamped pose_stamped;
147-
pose_stamped.header.frame_id = (costmap_ros_->getGlobalFrameID()).c_str();
148-
149-
// position & orientation
150-
pose_stamped.pose = utils::createPose(optimal_traj(i, 0), optimal_traj(i, 1), 0.);
151-
152-
tf2::Quaternion quaternion_tf2;
153-
quaternion_tf2.setRPY(0., 0., optimal_traj(i, 2));
154-
auto quaternion = tf2::toMsg(quaternion_tf2);
155-
pose_stamped.pose.orientation = quaternion;
156-
157-
// add pose to the path
158-
optimal_path.poses.push_back(pose_stamped);
159-
}
160-
optimal_path_pub_->publish(optimal_path);
161-
}
162-
163125
} // namespace nav2_mppi_controller
164126

165127
#include "pluginlib/class_list_macros.hpp"

‎nav2_mppi_controller/src/trajectory_visualizer.cpp‎

Lines changed: 41 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ void TrajectoryVisualizer::on_configure(
2828
trajectories_publisher_ =
2929
node->create_publisher<visualization_msgs::msg::MarkerArray>("/trajectories", 1);
3030
transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
31+
optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("/local_plan", 1);
3132
parameters_handler_ = parameters_handler;
3233

3334
auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
@@ -42,18 +43,21 @@ void TrajectoryVisualizer::on_cleanup()
4243
{
4344
trajectories_publisher_.reset();
4445
transformed_path_pub_.reset();
46+
optimal_path_pub_.reset();
4547
}
4648

4749
void TrajectoryVisualizer::on_activate()
4850
{
4951
trajectories_publisher_->on_activate();
5052
transformed_path_pub_->on_activate();
53+
optimal_path_pub_->on_activate();
5154
}
5255

5356
void TrajectoryVisualizer::on_deactivate()
5457
{
5558
trajectories_publisher_->on_deactivate();
5659
transformed_path_pub_->on_deactivate();
60+
optimal_path_pub_->on_deactivate();
5761
}
5862

5963
void TrajectoryVisualizer::add(
@@ -81,6 +85,8 @@ void TrajectoryVisualizer::add(
8185
for (size_t i = 0; i < size; i++) {
8286
add_marker(i);
8387
}
88+
89+
optimal_traj_ = std::make_unique<xt::xtensor<float, 2>>(trajectory);
8490
}
8591

8692
void TrajectoryVisualizer::add(
@@ -111,14 +117,19 @@ void TrajectoryVisualizer::reset()
111117
{
112118
marker_id_ = 0;
113119
points_ = std::make_unique<visualization_msgs::msg::MarkerArray>();
120+
optimal_traj_ = std::make_unique<xt::xtensor<float, 2>>();
114121
}
115122

116-
void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
123+
void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan, const builtin_interfaces::msg::Time & cmd_stamp)
117124
{
118125
if (trajectories_publisher_->get_subscription_count() > 0) {
119126
trajectories_publisher_->publish(std::move(points_));
120127
}
121128

129+
if (optimal_path_pub_->get_subscription_count() > 0) {
130+
publish_optimal_path(cmd_stamp);
131+
}
132+
122133
reset();
123134

124135
if (transformed_path_pub_->get_subscription_count() > 0) {
@@ -127,4 +138,33 @@ void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
127138
}
128139
}
129140

141+
void TrajectoryVisualizer::publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp)
142+
{
143+
auto & size = optimal_traj_->shape()[0];
144+
if (size == 0) {
145+
return;
146+
}
147+
148+
nav_msgs::msg::Path optimal_path;
149+
optimal_path.header.stamp = cmd_stamp;
150+
optimal_path.header.frame_id = frame_id_;
151+
152+
for (size_t i = 0; i < size; i++) {
153+
// create new pose for the path
154+
geometry_msgs::msg::PoseStamped pose_stamped;
155+
pose_stamped.header.frame_id = frame_id_;
156+
157+
// position & orientation
158+
pose_stamped.pose = utils::createPose((*optimal_traj_)(i, 0), (*optimal_traj_)(i, 1), 0.0);
159+
160+
tf2::Quaternion quaternion_tf2;
161+
quaternion_tf2.setRPY(0., 0., (*optimal_traj_)(i, 2));
162+
pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2);
163+
164+
// add pose to the path
165+
optimal_path.poses.push_back(pose_stamped);
166+
}
167+
optimal_path_pub_->publish(optimal_path);
168+
}
169+
130170
} // namespace mppi

0 commit comments

Comments
 (0)