Skip to content

Commit a98d3f1

Browse files
alyquantillionalyquantillion
committed
populate optimal path message in add()
Signed-off-by: Alyssa Agnissan <[email protected]>
1 parent 85e4d5a commit a98d3f1

File tree

4 files changed

+39
-37
lines changed

4 files changed

+39
-37
lines changed

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

Lines changed: 8 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,9 @@ class TrajectoryVisualizer
7979
* @brief Add an optimal trajectory to visualize
8080
* @param trajectory Optimal trajectory
8181
*/
82-
void add(const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace);
82+
void add(
83+
const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace,
84+
const builtin_interfaces::msg::Time & cmd_stamp);
8385

8486
/**
8587
* @brief Add candidate trajectories to visualize
@@ -91,13 +93,15 @@ class TrajectoryVisualizer
9193
* @brief Publish the optimal trajectory in the form of a path message
9294
* @param trajectory Optimal trajectory
9395
*/
94-
void publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp);
96+
void populate_optimal_path(
97+
const xt::xtensor<float, 2> & optimal_traj,
98+
const builtin_interfaces::msg::Time & cmd_stamp);
9599

96100
/**
97101
* @brief Visualize the plan
98102
* @param plan Plan to visualize
99103
*/
100-
void visualize(const nav_msgs::msg::Path & plan, const builtin_interfaces::msg::Time & cmd_stamp);
104+
void visualize(const nav_msgs::msg::Path & plan);
101105

102106
/**
103107
* @brief Reset object
@@ -111,7 +115,7 @@ class TrajectoryVisualizer
111115
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_path_pub_;
112116
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> optimal_path_pub_;
113117

114-
std::unique_ptr<xt::xtensor<float, 2>> optimal_traj_;
118+
std::unique_ptr<nav_msgs::msg::Path> optimal_path_;
115119
std::unique_ptr<visualization_msgs::msg::MarkerArray> points_;
116120
int marker_id_ = 0;
117121

‎nav2_mppi_controller/src/controller.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -110,8 +110,8 @@ void MPPIController::visualize(
110110
const builtin_interfaces::msg::Time & cmd_stamp)
111111
{
112112
trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
113-
trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
114-
trajectory_visualizer_.visualize(std::move(transformed_plan), cmd_stamp);
113+
trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory", cmd_stamp);
114+
trajectory_visualizer_.visualize(std::move(transformed_plan));
115115
}
116116

117117
void MPPIController::setPlan(const nav_msgs::msg::Path & path)

‎nav2_mppi_controller/src/trajectory_visualizer.cpp‎

Lines changed: 17 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -28,7 +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);
31+
optimal_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("optimal_trajectory", 1);
3232
parameters_handler_ = parameters_handler;
3333

3434
auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
@@ -61,7 +61,9 @@ void TrajectoryVisualizer::on_deactivate()
6161
}
6262

6363
void TrajectoryVisualizer::add(
64-
const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace)
64+
const xt::xtensor<float, 2> & trajectory,
65+
const std::string & marker_namespace,
66+
const builtin_interfaces::msg::Time & cmd_stamp)
6567
{
6668
auto & size = trajectory.shape()[0];
6769
if (!size) {
@@ -86,7 +88,7 @@ void TrajectoryVisualizer::add(
8688
add_marker(i);
8789
}
8890

89-
optimal_traj_ = std::make_unique<xt::xtensor<float, 2>>(trajectory);
91+
populate_optimal_path(trajectory, cmd_stamp);
9092
}
9193

9294
void TrajectoryVisualizer::add(
@@ -117,19 +119,17 @@ void TrajectoryVisualizer::reset()
117119
{
118120
marker_id_ = 0;
119121
points_ = std::make_unique<visualization_msgs::msg::MarkerArray>();
120-
optimal_traj_ = std::make_unique<xt::xtensor<float, 2>>();
122+
optimal_path_ = std::make_unique<nav_msgs::msg::Path>();
121123
}
122124

123-
void TrajectoryVisualizer::visualize(
124-
const nav_msgs::msg::Path & plan,
125-
const builtin_interfaces::msg::Time & cmd_stamp)
125+
void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
126126
{
127127
if (trajectories_publisher_->get_subscription_count() > 0) {
128128
trajectories_publisher_->publish(std::move(points_));
129129
}
130130

131131
if (optimal_path_pub_->get_subscription_count() > 0) {
132-
publish_optimal_path(cmd_stamp);
132+
optimal_path_pub_->publish(std::move(optimal_path_));
133133
}
134134

135135
reset();
@@ -140,33 +140,33 @@ void TrajectoryVisualizer::visualize(
140140
}
141141
}
142142

143-
void TrajectoryVisualizer::publish_optimal_path(const builtin_interfaces::msg::Time & cmd_stamp)
143+
void TrajectoryVisualizer::populate_optimal_path(
144+
const xt::xtensor<float, 2> & optimal_traj,
145+
const builtin_interfaces::msg::Time & cmd_stamp)
144146
{
145-
auto & size = optimal_traj_->shape()[0];
147+
auto & size = optimal_traj.shape()[0];
146148
if (size == 0) {
147149
return;
148150
}
149151

150-
nav_msgs::msg::Path optimal_path;
151-
optimal_path.header.stamp = cmd_stamp;
152-
optimal_path.header.frame_id = frame_id_;
152+
optimal_path_->header.stamp = cmd_stamp;
153+
optimal_path_->header.frame_id = frame_id_;
153154

154155
for (size_t i = 0; i < size; i++) {
155156
// create new pose for the path
156157
geometry_msgs::msg::PoseStamped pose_stamped;
157158
pose_stamped.header.frame_id = frame_id_;
158159

159160
// position & orientation
160-
pose_stamped.pose = utils::createPose((*optimal_traj_)(i, 0), (*optimal_traj_)(i, 1), 0.0);
161+
pose_stamped.pose = utils::createPose(optimal_traj(i, 0), optimal_traj(i, 1), 0.0);
161162

162163
tf2::Quaternion quaternion_tf2;
163-
quaternion_tf2.setRPY(0., 0., (*optimal_traj_)(i, 2));
164+
quaternion_tf2.setRPY(0., 0., optimal_traj(i, 2));
164165
pose_stamped.pose.orientation = tf2::toMsg(quaternion_tf2);
165166

166167
// add pose to the path
167-
optimal_path.poses.push_back(pose_stamped);
168+
optimal_path_->poses.push_back(pose_stamped);
168169
}
169-
optimal_path_pub_->publish(optimal_path);
170170
}
171171

172172
} // namespace mppi

‎nav2_mppi_controller/test/trajectory_visualizer_tests.cpp‎

Lines changed: 12 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -59,8 +59,7 @@ TEST(TrajectoryVisualizerTests, VisPathRepub)
5959
TrajectoryVisualizer vis;
6060
vis.on_configure(node, "my_name", "map", parameters_handler.get());
6161
vis.on_activate();
62-
builtin_interfaces::msg::Time bogus_stamp;
63-
vis.visualize(pub_path, bogus_stamp);
62+
vis.visualize(pub_path);
6463

6564
rclcpp::spin_some(node->get_node_base_interface());
6665
EXPECT_EQ(recieved_path.poses.size(), 5u);
@@ -82,18 +81,18 @@ TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
8281
TrajectoryVisualizer vis;
8382
vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
8483
vis.on_activate();
85-
vis.add(optimal_trajectory, "Optimal Trajectory");
86-
nav_msgs::msg::Path bogus_path;
8784
builtin_interfaces::msg::Time bogus_stamp;
88-
vis.visualize(bogus_path, bogus_stamp);
85+
vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
86+
nav_msgs::msg::Path bogus_path;
87+
vis.visualize(bogus_path);
8988

9089
rclcpp::spin_some(node->get_node_base_interface());
9190
EXPECT_EQ(recieved_msg.markers.size(), 0u);
9291

9392
// Now populated with content, should publish
9493
optimal_trajectory = xt::ones<float>({20, 2});
95-
vis.add(optimal_trajectory, "Optimal Trajectory");
96-
vis.visualize(bogus_path, bogus_stamp);
94+
vis.add(optimal_trajectory, "Optimal Trajectory", bogus_stamp);
95+
vis.visualize(bogus_path);
9796

9897
rclcpp::spin_some(node->get_node_base_interface());
9998

@@ -149,8 +148,7 @@ TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
149148
vis.on_activate();
150149
vis.add(candidate_trajectories, "Candidate Trajectories");
151150
nav_msgs::msg::Path bogus_path;
152-
builtin_interfaces::msg::Time bogus_stamp;
153-
vis.visualize(bogus_path, bogus_stamp);
151+
vis.visualize(bogus_path);
154152

155153
rclcpp::spin_some(node->get_node_base_interface());
156154
// 40 * 4, for 5 trajectory steps + 3 point steps
@@ -167,17 +165,17 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
167165

168166
nav_msgs::msg::Path recieved_path;
169167
auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
170-
"/local_plan", 10,
168+
"optimal_trajectory", 10,
171169
[&](const nav_msgs::msg::Path msg) {recieved_path = msg;});
172170

173171
// optimal_trajectory empty, should fail to publish
174172
xt::xtensor<float, 2> optimal_trajectory;
175173
TrajectoryVisualizer vis;
176174
vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
177175
vis.on_activate();
178-
vis.add(optimal_trajectory, "Optimal Trajectory");
176+
vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
179177
nav_msgs::msg::Path bogus_path;
180-
vis.visualize(bogus_path, cmd_stamp);
178+
vis.visualize(bogus_path);
181179

182180
rclcpp::spin_some(node->get_node_base_interface());
183181
EXPECT_EQ(recieved_path.poses.size(), 0u);
@@ -188,8 +186,8 @@ TEST(TrajectoryVisualizerTests, VisOptimalPath)
188186
optimal_trajectory(i, 0) = static_cast<float>(i);
189187
optimal_trajectory(i, 1) = static_cast<float>(i);
190188
}
191-
vis.add(optimal_trajectory, "Optimal Trajectory");
192-
vis.visualize(bogus_path, cmd_stamp);
189+
vis.add(optimal_trajectory, "Optimal Trajectory", cmd_stamp);
190+
vis.visualize(bogus_path);
193191

194192
rclcpp::spin_some(node->get_node_base_interface());
195193

0 commit comments

Comments
 (0)