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

Feature/ground segmentation #78

Merged
merged 5 commits into from
Nov 14, 2023
Merged

Feature/ground segmentation #78

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
2850ef8
add ground segmentation to launch and config
Alpaca-zip Nov 12, 2023
9732ee2
update dependencies
Alpaca-zip Nov 12, 2023
6b08352
fix to explicitly specify the sensor frame
Alpaca-zip Nov 13, 2023
c397d99
fix bug with parameters not being loaded
Alpaca-zip Nov 14, 2023
c8fb308
Merge branch 'feature/ground_segmentation' of github.com:KBKN-Autonom…
Alpaca-zip Nov 14, 2023
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
9 changes: 8 additions & 1 deletion orange_bringup/launch/orange_robot.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,9 +107,16 @@
<param name="serial_interval" value="0.01"/>
<remap from="/estop/state" to="/estop"/>
</node>
<!-- ground_segmentation -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/ground_segmentation.launch.xml">
<arg name="config_file_path" value="$(find-pkg-share orange_sensor_tools)/config/ground_segmentation.yaml"/>
<arg name="input_topic" value="$(var velodyne_points_topic)"/>
<arg name="ground_output_topic" value="$(var velodyne_points_topic)/ground"/>
<arg name="obstacle_output_topic" value="$(var velodyne_points_topic)/obstacle"/>
</include>
<!-- pointcloud_to_laserscan -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/pointcloud_to_laserscan.launch.xml">
<arg name="cloud_in" value="$(var velodyne_points_topic)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)/obstacle"/>
<arg name="scan_out" value="$(var velodyne_scan_topic)"/>
</include>
<!-- laserscan_multi_merger -->
Expand Down
10 changes: 9 additions & 1 deletion orange_bringup/launch/with_ros2bag.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,10 +11,18 @@
<arg name="imu_topic" default="/imu"/>
<arg name="fusion_odom_topic" default="/fusion/odom"/>
<arg name="xacro_file_path" default="$(find-pkg-share orange_description)/xacro/orange_robot.xacro"/>
<!-- ground_segmentation -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/ground_segmentation.launch.xml">
<arg name="config_file_path" value="$(find-pkg-share orange_sensor_tools)/config/ground_segmentation.yaml"/>
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="input_topic" value="$(var velodyne_points_topic)"/>
<arg name="ground_output_topic" value="$(var velodyne_points_topic)/ground"/>
<arg name="obstacle_output_topic" value="$(var velodyne_points_topic)/obstacle"/>
</include>
<!-- pointcloud_to_laserscan -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/pointcloud_to_laserscan.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)/obstacle"/>
<arg name="scan_out" value="$(var velodyne_scan_topic)"/>
</include>
<!-- laserscan_multi_merger -->
Expand Down
9 changes: 8 additions & 1 deletion orange_gazebo/launch/empty_world.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,17 @@
<arg name="fusion_odom_topic" default="/fusion/odom"/>
<arg name="world_file_path" default="$(find-pkg-share gazebo_ros)/worlds/empty.world"/>
<arg name="xacro_file_path" default="$(find-pkg-share orange_description)/xacro/orange_robot.xacro"/>
<!-- ground_segmentation -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/ground_segmentation.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="input_topic" value="$(var velodyne_points_topic)"/>
<arg name="ground_output_topic" value="$(var velodyne_points_topic)/ground"/>
<arg name="obstacle_output_topic" value="$(var velodyne_points_topic)/obstacle"/>
</include>
<!-- pointcloud_to_laserscan -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/pointcloud_to_laserscan.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)/obstacle"/>
<arg name="scan_out" value="$(var velodyne_scan_topic)"/>
</include>
<!-- laserscan_multi_merger -->
Expand Down
9 changes: 8 additions & 1 deletion orange_gazebo/launch/orange_hosei.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,10 +15,17 @@
<arg name="x" default="0.0"/>
<arg name="y" default="0.0"/>
<arg name="yaw" default="0.0"/>
<!-- ground_segmentation -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/ground_segmentation.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="input_topic" value="$(var velodyne_points_topic)"/>
<arg name="ground_output_topic" value="$(var velodyne_points_topic)/ground"/>
<arg name="obstacle_output_topic" value="$(var velodyne_points_topic)/obstacle"/>
</include>
<!-- pointcloud_to_laserscan -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/pointcloud_to_laserscan.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)/obstacle"/>
<arg name="scan_out" value="$(var velodyne_scan_topic)"/>
</include>
<!-- laserscan_multi_merger -->
Expand Down
9 changes: 8 additions & 1 deletion orange_gazebo/launch/orange_igvc.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,17 @@
<arg name="fusion_odom_topic" default="/fusion/odom"/>
<arg name="world_file_path" default="$(find-pkg-share orange_gazebo)/worlds/orange_igvc.world"/>
<arg name="xacro_file_path" default="$(find-pkg-share orange_description)/xacro/orange_robot.xacro"/>
<!-- ground_segmentation -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/ground_segmentation.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="input_topic" value="$(var velodyne_points_topic)"/>
<arg name="ground_output_topic" value="$(var velodyne_points_topic)/ground"/>
<arg name="obstacle_output_topic" value="$(var velodyne_points_topic)/obstacle"/>
</include>
<!-- pointcloud_to_laserscan -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/pointcloud_to_laserscan.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)/obstacle"/>
<arg name="scan_out" value="$(var velodyne_scan_topic)"/>
</include>
<!-- laserscan_multi_merger -->
Expand Down
9 changes: 8 additions & 1 deletion orange_gazebo/launch/orange_world.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,17 @@
<arg name="fusion_odom_topic" default="/fusion/odom"/>
<arg name="world_file_path" default="$(find-pkg-share orange_gazebo)/worlds/orange_world.world"/>
<arg name="xacro_file_path" default="$(find-pkg-share orange_description)/xacro/orange_robot.xacro"/>
<!-- ground_segmentation -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/ground_segmentation.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="input_topic" value="$(var velodyne_points_topic)"/>
<arg name="ground_output_topic" value="$(var velodyne_points_topic)/ground"/>
<arg name="obstacle_output_topic" value="$(var velodyne_points_topic)/obstacle"/>
</include>
<!-- pointcloud_to_laserscan -->
<include file="$(find-pkg-share orange_sensor_tools)/launch/pointcloud_to_laserscan.launch.xml">
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)"/>
<arg name="cloud_in" value="$(var velodyne_points_topic)/obstacle"/>
<arg name="scan_out" value="$(var velodyne_scan_topic)"/>
</include>
<!-- laserscan_multi_merger -->
Expand Down
4 changes: 4 additions & 0 deletions orange_ros2.rosinstall
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,3 +22,7 @@
local-name: multi_map_manager
uri: https://github.com/KBKN-Autonomous-Robotics-Lab/multi_map_manager.git
version: humble-devel
- git:
local-name: linefit_ground_segmentation_ros2
uri: https://github.com/KBKN-Autonomous-Robotics-Lab/linefit_ground_segmentation_ros2.git
version: main
23 changes: 23 additions & 0 deletions orange_sensor_tools/config/ground_segmentation.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
ground_segmentation:
ros__parameters:
n_threads: 1 # number of threads to use.

r_min: 0.9 # minimum point distance.
r_max: 100.0 # maximum point distance.
n_bins: 200 # number of radial bins.
n_segments: 720 # number of radial segments.

max_dist_to_line: 0.15 # maximum vertical distance of point to line to be considered ground.

sensor_height: 1.0 # sensor height above ground.
min_slope: 0.0 # minimum slope of a ground line.
max_slope: 3.0 # maximum slope of a ground line.
max_fit_error: 0.07 # maximum error of a point during line fit.
long_threshold: 10.0 # distance between points after which they are considered far from each other.
max_long_height: 0.3 # maximum height change to previous point in long line.
max_start_height: 0.5 # maximum difference to estimated ground height to start a new line.
line_search_angle: 0.2 # how far to search in angular direction to find a line [rad].

gravity_aligned_frame: "velodyne" # Frame which has its z axis aligned with gravity.

visualize: false # visualize segmentation result - USE ONLY FOR DEBUGGING
16 changes: 16 additions & 0 deletions orange_sensor_tools/launch/ground_segmentation.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
<?xml version="1.0"?>
<launch>
<arg name="use_sim_time" default="false"/>
<arg name="config_file_path" default="$(find-pkg-share orange_sensor_tools)/config/ground_segmentation.yaml"/>
<arg name="input_topic" default="/velodyne_points"/>
<arg name="ground_output_topic" default="/velodyne_points/ground"/>
<arg name="obstacle_output_topic" default="/velodyne_points/obstacle"/>
<!-- ground_segmentation -->
<node pkg="linefit_ground_segmentation_ros" exec="ground_segmentation_node">
<param name="use_sim_time" value="$(var use_sim_time)"/>
<param from="$(var config_file_path)"/>
<param name="input_topic" value="$(var input_topic)"/>
<param name="ground_output_topic" value="$(var ground_output_topic)"/>
<param name="obstacle_output_topic" value="$(var obstacle_output_topic)"/>
</node>
</launch>
1 change: 1 addition & 0 deletions orange_sensor_tools/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
<depend>pointcloud_to_laserscan</depend>
<depend>robot_localization</depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<exec_depend>linefit_ground_segmentation_ros</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
Loading