Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

chore(freespace_planning_algorithms): update for A* python wrapper #8890

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
+32 −11
Open

chore(freespace_planning_algorithms): update for A* python wrapper #8890

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1853f8d
add get distance to obstacle function in the pyrhon wrapper
Aug 9, 2024
b26fe83
add setMinMaxDimension Function
Aug 23, 2024
d2226de
run pre-commit
Oct 9, 2024
7ebb6b9
Merge branch 'main' into feat/astar_wrapper_distance_function
mitsudome-r Feb 4, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions ...ware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,3 +71,7 @@ def getWaypoints(self):
)

return waypoints

def getDistanceToObstacle(self, pose: Pose):
pose_byte = serialize_message(pose)
return self.astar_search.getDistanceToObstacle(pose_byte)
39 changes: 28 additions & 11 deletions planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,20 +99,35 @@
waypoints_vector.length = waypoints.compute_length();
return waypoints_vector;
}

double getDistanceToObstacle(const std::string & pose_byte)

Check warning on line 103 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L103 

Added line #L103 was not covered by tests
{
rclcpp::SerializedMessage serialized_msg;

Check warning on line 105 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L105 

Added line #L105 was not covered by tests
static constexpr size_t message_header_length = 8u;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

how to know this value 8u?

Copy link
Contributor Author

@TakumIto TakumIto Oct 10, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I imitated the code of another feature.
https://github.com/autowarefoundation/autoware_common/blob/7c55915cb435c17f24f78dd9b9f28fa269609671/tmp/lanelet2_extension_python/src/utility.cpp#L136
It uses the same header length 8u for all messages. But I'm sorry, I have no idea how it has been decided.

serialized_msg.reserve(message_header_length + pose_byte.size());
serialized_msg.get_rcl_serialized_message().buffer_length = pose_byte.size();

Check warning on line 108 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L107-L108 

Added lines #L107 - L108 were not covered by tests
for (size_t i = 0; i < pose_byte.size(); ++i) {
serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];

Check warning on line 110 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L110 

Added line #L110 was not covered by tests
}
geometry_msgs::msg::Pose pose;
static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
serializer.deserialize_message(&serialized_msg, &pose);

Check warning on line 114 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L113-L114 

Added lines #L113 - L114 were not covered by tests

return freespace_planning_algorithms::AstarSearch::getDistanceToObstacle(pose);
}

Check warning on line 117 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L116-L117 

Added lines #L116 - L117 were not covered by tests
};

namespace py = pybind11;

// cppcheck-suppress syntaxError
PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p)
{
auto pyPlannerWaypointsVector =
py::class_<PlannerWaypointsVector>(p, "PlannerWaypointsVector", py::dynamic_attr())
.def(py::init<>())
.def_readwrite("waypoints", &PlannerWaypointsVector::waypoints)
.def_readwrite("length", &PlannerWaypointsVector::length);
auto pyPlannerWaypointsVector = py::class_<PlannerWaypointsVector>(p, "PlannerWaypointsVector")

Check warning on line 125 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L125 

Added line #L125 was not covered by tests
.def(py::init<>())
.def_readwrite("waypoints", &PlannerWaypointsVector::waypoints)
.def_readwrite("length", &PlannerWaypointsVector::length);

Check warning on line 128 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L127-L128 

Added lines #L127 - L128 were not covered by tests
auto pyAstarParam =
py::class_<freespace_planning_algorithms::AstarParam>(p, "AstarParam", py::dynamic_attr())
py::class_<freespace_planning_algorithms::AstarParam>(p, "AstarParam")

Check warning on line 130 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L130 

Added line #L130 was not covered by tests
.def(py::init<>())
.def_readwrite("search_method", &freespace_planning_algorithms::AstarParam::search_method)
.def_readwrite(
Expand All @@ -137,8 +152,7 @@
"goal_lat_distance_weight",
&freespace_planning_algorithms::AstarParam::goal_lat_distance_weight);
auto pyPlannerCommonParam =
py::class_<freespace_planning_algorithms::PlannerCommonParam>(
p, "PlannerCommonParam", py::dynamic_attr())
py::class_<freespace_planning_algorithms::PlannerCommonParam>(p, "PlannerCommonParam")

Check warning on line 155 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L155 

Added line #L155 was not covered by tests
.def(py::init<>())
.def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit)
.def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size)
Expand All @@ -165,14 +179,16 @@
"obstacle_threshold",
&freespace_planning_algorithms::PlannerCommonParam::obstacle_threshold);
auto pyVehicleShape =
py::class_<freespace_planning_algorithms::VehicleShape>(p, "VehicleShape", py::dynamic_attr())
py::class_<freespace_planning_algorithms::VehicleShape>(p, "VehicleShape")

Check warning on line 182 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L182 

Added line #L182 was not covered by tests
.def(py::init<>())
.def(py::init<double, double, double, double, double>())
.def("setMinMaxDimension", &freespace_planning_algorithms::VehicleShape::setMinMaxDimension)

Check warning on line 185 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L185 

Added line #L185 was not covered by tests
.def_readwrite("length", &freespace_planning_algorithms::VehicleShape::length)
.def_readwrite("width", &freespace_planning_algorithms::VehicleShape::width)
.def_readwrite("base_length", &freespace_planning_algorithms::VehicleShape::base_length)
.def_readwrite("max_steering", &freespace_planning_algorithms::VehicleShape::max_steering)
.def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back);
.def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back)
.def_readwrite("min_dimension", &freespace_planning_algorithms::VehicleShape::min_dimension);

Check warning on line 191 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L190-L191 

Added lines #L190 - L191 were not covered by tests

auto pyAbstractPlanningAlgorithm =
py::class_<freespace_planning_algorithms::AbstractPlanningAlgorithm>(
Expand All @@ -187,6 +203,7 @@
freespace_planning_algorithms::AstarParam &>())
.def("setMap", &AstarSearchPython::setMapByte)
.def("makePlan", &AstarSearchPython::makePlanByte)
.def("getWaypoints", &AstarSearchPython::getWaypointsAsVector);
.def("getWaypoints", &AstarSearchPython::getWaypointsAsVector)
.def("getDistanceToObstacle", &AstarSearchPython::getDistanceToObstacle);

Check warning on line 207 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

PYBIND11_MODULE increases from 84 to 85 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.

Check warning on line 207 in planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp#L206-L207 

Added lines #L206 - L207 were not covered by tests
}
} // namespace autoware::freespace_planning_algorithms
Loading