-
Notifications
You must be signed in to change notification settings - Fork 658
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
refactor(obstacle_collision_checker): move to autoware namespace (#9047)
Signed-off-by: Maxime CLEMENT <[email protected]>
- Loading branch information
1 parent
a3c7ede
commit 23f4c86
Showing
15 changed files
with
155 additions
and
110 deletions.
There are no files selected for viewing
Validating CODEOWNERS rules …
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
File renamed without changes.
32 changes: 32 additions & 0 deletions
32
...autoware_obstacle_collision_checker/include/autoware/obstacle_collision_checker/debug.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,32 @@ | ||
| // Copyright 2024 Tier IV, Inc. All rights reserved. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ | ||
| #define AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ | ||
|
|
||
| #include "autoware/obstacle_collision_checker/obstacle_collision_checker.hpp" | ||
|
|
||
| #include <visualization_msgs/msg/marker_array.hpp> | ||
|
|
||
| namespace autoware::obstacle_collision_checker | ||
| { | ||
| /// @brief create debug markers of the given output | ||
| /// @param output structure with output data calculated by the obstacle_collision_checker module | ||
| /// @param base_link_z current z value of the base_link in map frame | ||
| /// @param now current time | ||
| /// @return debug markers | ||
| visualization_msgs::msg::MarkerArray create_marker_array( | ||
| const Output & output, const double base_link_z, const rclcpp::Time & now); | ||
| } // namespace autoware::obstacle_collision_checker | ||
| #endif // AUTOWARE__OBSTACLE_COLLISION_CHECKER__DEBUG_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
2 changes: 1 addition & 1 deletion
2
...ol/obstacle_collision_checker/package.xml → ...re_obstacle_collision_checker/package.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,7 +1,7 @@ | ||
| <?xml version="1.0"?> | ||
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>obstacle_collision_checker</name> | ||
| <name>autoware_obstacle_collision_checker</name> | ||
| <version>0.1.0</version> | ||
| <description>The obstacle_collision_checker package</description> | ||
| <maintainer email="[email protected]">Taiki Tanaka</maintainer> | ||
|
|
||
File renamed without changes.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,89 @@ | ||
| // Copyright 2024 Tier IV, Inc. All rights reserved. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #include "autoware/obstacle_collision_checker/debug.hpp" | ||
|
|
||
| #include <autoware/universe_utils/ros/marker_helper.hpp> | ||
|
|
||
| namespace autoware::obstacle_collision_checker | ||
| { | ||
| visualization_msgs::msg::MarkerArray create_marker_array( | ||
| const Output & output, const double base_link_z, const rclcpp::Time & now) | ||
| { | ||
| using autoware::universe_utils::createDefaultMarker; | ||
| using autoware::universe_utils::createMarkerColor; | ||
| using autoware::universe_utils::createMarkerScale; | ||
|
|
||
| visualization_msgs::msg::MarkerArray marker_array; | ||
|
|
||
| if (output.resampled_trajectory.points.size() >= 2) { | ||
| // Line of resampled_trajectory | ||
| { | ||
| auto marker = createDefaultMarker( | ||
| "map", now, "resampled_trajectory_line", 0, visualization_msgs::msg::Marker::LINE_STRIP, | ||
| createMarkerScale(0.05, 0, 0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); | ||
|
|
||
| for (const auto & p : output.resampled_trajectory.points) { | ||
| marker.points.push_back(p.pose.position); | ||
| marker.colors.push_back(marker.color); | ||
| } | ||
|
|
||
| marker_array.markers.push_back(marker); | ||
| } | ||
|
|
||
| // Points of resampled_trajectory | ||
| { | ||
| auto marker = createDefaultMarker( | ||
| "map", now, "resampled_trajectory_points", 0, visualization_msgs::msg::Marker::SPHERE_LIST, | ||
| createMarkerScale(0.1, 0.1, 0.1), createMarkerColor(0.0, 1.0, 0.0, 0.999)); | ||
|
|
||
| for (const auto & p : output.resampled_trajectory.points) { | ||
| marker.points.push_back(p.pose.position); | ||
| marker.colors.push_back(marker.color); | ||
| } | ||
|
|
||
| marker_array.markers.push_back(marker); | ||
| } | ||
| } | ||
|
|
||
| // Vehicle Footprints | ||
| { | ||
| const auto color_ok = createMarkerColor(0.0, 1.0, 0.0, 0.5); | ||
| const auto color_will_collide = createMarkerColor(1.0, 0.0, 0.0, 0.5); | ||
|
|
||
| auto color = color_ok; | ||
| if (output.will_collide) { | ||
| color = color_will_collide; | ||
| } | ||
|
|
||
| auto marker = createDefaultMarker( | ||
| "map", now, "vehicle_footprints", 0, visualization_msgs::msg::Marker::LINE_LIST, | ||
| createMarkerScale(0.05, 0, 0), color); | ||
|
|
||
| for (const auto & vehicle_footprint : output.vehicle_footprints) { | ||
| for (size_t i = 0; i < vehicle_footprint.size() - 1; ++i) { | ||
| const auto & p1 = vehicle_footprint.at(i); | ||
| const auto & p2 = vehicle_footprint.at(i + 1); | ||
|
|
||
| marker.points.push_back(toMsg(p1.to_3d(base_link_z))); | ||
| marker.points.push_back(toMsg(p2.to_3d(base_link_z))); | ||
| } | ||
| } | ||
|
|
||
| marker_array.markers.push_back(marker); | ||
| } | ||
|
|
||
| return marker_array; | ||
| } | ||
| } // namespace autoware::obstacle_collision_checker |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.