Skip to content

Commit

Permalink
fix
Browse files Browse the repository at this point in the history
Signed-off-by: Tony Najjar <[email protected]>
  • Loading branch information
tonynajjar committed Jan 7, 2025
1 parent 80e0c17 commit 30aed4b
Show file tree
Hide file tree
Showing 4 changed files with 5 additions and 5 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ void RemoveInCollisionGoals::on_tick()
request_->use_footprint = use_footprint_;

for (const auto & goal : input_goals_.poses) {
request_->poses.poses.push_back(goal);
request_->poses.push_back(goal);
}
}

Expand Down
2 changes: 1 addition & 1 deletion nav2_costmap_2d/src/costmap_2d_ros.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -831,7 +831,7 @@ void Costmap2DROS::getCostsCallback(

Costmap2D * costmap = layered_costmap_->getCostmap();
response->success = true;
for (const auto & pose : request->poses.poses) {
for (const auto & pose : request->poses) {
geometry_msgs::msg::PoseStamped pose_transformed;
if (!transformPoseToGlobalFrame(pose, pose_transformed)) {
RCLCPP_ERROR(
Expand Down
4 changes: 2 additions & 2 deletions nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint)
geometry_msgs::msg::PoseStamped pose;
pose.pose.position.x = 0.5;
pose.pose.position.y = 1.0;
request->poses.poses.push_back(pose);
request->poses.push_back(pose);
request->use_footprint = false;

auto result_future = client_->async_send_request(request);
Expand All @@ -78,7 +78,7 @@ TEST_F(GetCostServiceTest, TestWithFootprint)
tf2::Quaternion q;
q.setRPY(0, 0, 0.5);
pose.pose.orientation = tf2::toMsg(q);
request->poses.poses.push_back(pose);
request->poses.push_back(pose);
request->use_footprint = true;

auto result_future = client_->async_send_request(request);
Expand Down
2 changes: 1 addition & 1 deletion nav2_rviz_plugins/src/costmap_cost_tool.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ void CostmapCostTool::callCostService(float x, float y)
pose.header.stamp = node->now();
pose.pose.position.x = x;
pose.pose.position.y = y;
request->poses.poses.push_back(pose);
request->poses.push_back(pose);
request->use_footprint = false;

// Call local costmap service
Expand Down

0 comments on commit 30aed4b

Please sign in to comment.