DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HiepLM update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 09:08:14 +07:00
parent 71adf1390f
commit 2c3d7d586d
23 changed files with 117 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
plugins/directional_layer.cpp
View File

@@ -19,7 +19,7 @@ namespace costmap_2d
DirectionalLayer::~DirectionalLayer() {}
bool DirectionalLayer::laneFilter(const std::vector<geometry_msgs::PoseStamped> plan)
bool DirectionalLayer::laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan)
{
if (directional_areas_.empty())
throw "Has no map data";
@@ -119,13 +119,13 @@ namespace costmap_2d
std::vector<std::pair<unsigned int, double>> X_List, Y_List;
std::vector<double> Yaw_list;
const geometry_msgs::PoseStamped &e = path.poses.back();
const robot_geometry_msgs::PoseStamped &e = path.poses.back();
bool get_success = getRobotPose(pose_x_, pose_y_, pose_yaw_);
if(!get_success) return false;
for (auto it = path.poses.begin(); it != path.poses.end(); ++it)
{
const geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
const robot_geometry_msgs::PoseStamped &p = *it; // Get the pose from the iterator
unsigned int mx, my;
if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
{