Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
R
robinspec_sim
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package registry
Container Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Lenny Siol
robinspec_sim
Commits
7c6fea71
Commit
7c6fea71
authored
2 weeks ago
by
Lenny Siol
Browse files
Options
Downloads
Patches
Plain Diff
start implementing pointcloud to costmap processing
parent
6ce4d123
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
robinspec_pointcloud_to_costmap/CMakeLists.txt
+4
-1
4 additions, 1 deletion
robinspec_pointcloud_to_costmap/CMakeLists.txt
robinspec_pointcloud_to_costmap/src/rptc.cpp
+52
-4
52 additions, 4 deletions
robinspec_pointcloud_to_costmap/src/rptc.cpp
with
56 additions
and
5 deletions
robinspec_pointcloud_to_costmap/CMakeLists.txt
+
4
−
1
View file @
7c6fea71
...
@@ -13,6 +13,9 @@ find_package(laser_geometry REQUIRED)
...
@@ -13,6 +13,9 @@ find_package(laser_geometry REQUIRED)
find_package
(
tf2_ros REQUIRED
)
find_package
(
tf2_ros REQUIRED
)
find_package
(
pcl_ros REQUIRED
)
find_package
(
pcl_ros REQUIRED
)
find_package
(
pcl_conversions REQUIRED
)
find_package
(
pcl_conversions REQUIRED
)
find_package
(
nav2_costmap_2d REQUIRED
)
find_package
(
nav_msgs REQUIRED
)
find_package
(
std_msgs REQUIRED
)
add_executable
(
rptc src/rptc.cpp
)
add_executable
(
rptc src/rptc.cpp
)
target_include_directories
(
rptc PUBLIC
target_include_directories
(
rptc PUBLIC
...
@@ -20,7 +23,7 @@ target_include_directories(rptc PUBLIC
...
@@ -20,7 +23,7 @@ target_include_directories(rptc PUBLIC
$<INSTALL_INTERFACE:include/
${
PROJECT_NAME
}
>
)
$<INSTALL_INTERFACE:include/
${
PROJECT_NAME
}
>
)
target_compile_features
(
rptc PUBLIC c_std_99 cxx_std_17
)
# Require C99 and C++17
target_compile_features
(
rptc PUBLIC c_std_99 cxx_std_17
)
# Require C99 and C++17
ament_target_dependencies
(
rptc rclcpp sensor_msgs laser_geometry tf2_ros pcl_ros pcl_conversions
)
ament_target_dependencies
(
rptc rclcpp sensor_msgs laser_geometry tf2_ros pcl_ros pcl_conversions
nav2_costmap_2d nav_msgs std_msgs
)
install
(
TARGETS rptc
install
(
TARGETS rptc
DESTINATION lib/
${
PROJECT_NAME
}
)
DESTINATION lib/
${
PROJECT_NAME
}
)
...
...
This diff is collapsed.
Click to expand it.
robinspec_pointcloud_to_costmap/src/rptc.cpp
+
52
−
4
View file @
7c6fea71
...
@@ -10,6 +10,9 @@
...
@@ -10,6 +10,9 @@
#include
<pcl/features/normal_3d.h>
#include
<pcl/features/normal_3d.h>
#include
<string>
#include
<string>
#include
<vector>
#include
<vector>
#include
"nav2_costmap_2d/costmap_2d_ros.hpp"
#include
"nav_msgs/msg/occupancy_grid.hpp"
#include
"std_msgs/msg/header.hpp"
class
point_cloud_to_costmap
:
public
rclcpp
::
Node
{
class
point_cloud_to_costmap
:
public
rclcpp
::
Node
{
public:
public:
...
@@ -87,11 +90,56 @@ private:
...
@@ -87,11 +90,56 @@ private:
point
.
x
,
point
.
y
,
point
.
z
,
point
.
normal_x
,
point
.
normal_y
,
point
.
normal_z
);
point
.
x
,
point
.
y
,
point
.
z
,
point
.
normal_x
,
point
.
normal_y
,
point
.
normal_z
);
}
}
// Create costmap with 5x5 meters 100x100 cells
nav_msgs
::
msg
::
OccupancyGrid
costmap
=
init_costmap
(
0.05
,
100
,
100
);
// Create 2d grid for gradients
PointNT
arr
[
101
][
101
];
// Align to grid
for
(
const
auto
&
point
:
cloud_with_normals
->
points
)
{
}
// Sum up
for
(
const
auto
&
point
:
cloud_with_normals
->
points
)
{
}
// Normalize
for
(
const
auto
&
point
:
cloud_with_normals
->
points
)
{
}
// Calculate gradients between cells
// Transfer to costmap
// Create grid around robot
// Create grid around robot
// Align points to grid
// Average normals in each grid cell
// Calculate gradient between grid cells
// set data for costmap: costmap.data[index] = 100
// Return 2D gradient map
}
nav_msgs
::
msg
::
OccupancyGrid
init_costmap
(
float
resolution_in_meters
,
int
width
,
int
height
)
{
nav_msgs
::
msg
::
OccupancyGrid
costmap
;
// Set the header
costmap
.
header
.
stamp
=
rclcpp
::
Clock
().
now
();
costmap
.
header
.
frame_id
=
"map"
;
// Define the costmap properties
costmap
.
info
.
resolution
=
resolution_in_meters
;
// Resolution in meters per cell
costmap
.
info
.
width
=
width
;
// Width in cells
costmap
.
info
.
height
=
height
;
// Height in cells
costmap
.
info
.
origin
.
position
.
x
=
0.0
;
// Origin of the costmap
costmap
.
info
.
origin
.
position
.
y
=
0.0
;
costmap
.
info
.
origin
.
position
.
z
=
0.0
;
costmap
.
info
.
origin
.
orientation
.
w
=
1.0
;
// Initialize the costmap data with unknown values (-1)
costmap
.
data
.
resize
(
costmap
.
info
.
width
*
costmap
.
info
.
height
,
-
1
);
return
costmap
;
}
}
};
};
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment