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 3 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
3 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
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 @@ -96,93 +96,110 @@
waypoints_vector.length = waypoints.compute_length();
return waypoints_vector;
}

double getDistanceToObstacle(const std::string & pose_byte)
{
rclcpp::SerializedMessage serialized_msg;
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();
for (size_t i = 0; i < pose_byte.size(); ++i) {
serialized_msg.get_rcl_serialized_message().buffer[i] = pose_byte[i];
}
geometry_msgs::msg::Pose pose;
static rclcpp::Serialization<geometry_msgs::msg::Pose> serializer;
serializer.deserialize_message(&serialized_msg, &pose);

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

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")
.def(py::init<>())
.def_readwrite("waypoints", &PlannerWaypointsVector::waypoints)
.def_readwrite("length", &PlannerWaypointsVector::length);
auto pyAstarParam =
py::class_<freespace_planning_algorithms::AstarParam>(p, "AstarParam", py::dynamic_attr())
py::class_<freespace_planning_algorithms::AstarParam>(p, "AstarParam")
.def(py::init<>())
.def_readwrite("search_method", &freespace_planning_algorithms::AstarParam::search_method)
.def_readwrite(
"only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions)
.def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back)
.def_readwrite(
"adapt_expansion_distance",
&freespace_planning_algorithms::AstarParam::adapt_expansion_distance)
.def_readwrite(
"expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance)
.def_readwrite(
"near_goal_distance", &freespace_planning_algorithms::AstarParam::near_goal_distance)
.def_readwrite(
"distance_heuristic_weight",
&freespace_planning_algorithms::AstarParam::distance_heuristic_weight)
.def_readwrite(
"smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight)
.def_readwrite(
"obstacle_distance_weight",
&freespace_planning_algorithms::AstarParam::obstacle_distance_weight)
.def_readwrite(
"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")
.def(py::init<>())
.def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit)
.def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size)
.def_readwrite(
"max_turning_ratio", &freespace_planning_algorithms::PlannerCommonParam::max_turning_ratio)
.def_readwrite(
"turning_steps", &freespace_planning_algorithms::PlannerCommonParam::turning_steps)
.def_readwrite(
"curve_weight", &freespace_planning_algorithms::PlannerCommonParam::curve_weight)
.def_readwrite(
"reverse_weight", &freespace_planning_algorithms::PlannerCommonParam::reverse_weight)
.def_readwrite(
"direction_change_weight",
&freespace_planning_algorithms::PlannerCommonParam::direction_change_weight)
.def_readwrite(
"lateral_goal_range",
&freespace_planning_algorithms::PlannerCommonParam::lateral_goal_range)
.def_readwrite(
"longitudinal_goal_range",
&freespace_planning_algorithms::PlannerCommonParam::longitudinal_goal_range)
.def_readwrite(
"angle_goal_range", &freespace_planning_algorithms::PlannerCommonParam::angle_goal_range)
.def_readwrite(
"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")
.def(py::init<>())
.def(py::init<double, double, double, double, double>())
.def("setMinMaxDimension", &freespace_planning_algorithms::VehicleShape::setMinMaxDimension)
.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);

py::class_<freespace_planning_algorithms::AbstractPlanningAlgorithm>(
p, "AbstractPlanningAlgorithm");
py::class_<
freespace_planning_algorithms::AstarSearch,
freespace_planning_algorithms::AbstractPlanningAlgorithm>(p, "AstarSearchCpp");
py::class_<AstarSearchPython, freespace_planning_algorithms::AstarSearch>(p, "AstarSearch")
.def(py::init<
freespace_planning_algorithms::PlannerCommonParam &,
freespace_planning_algorithms::VehicleShape &,
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 203 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 83 to 84 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.
}
} // namespace autoware::freespace_planning_algorithms
Loading