-
Notifications
You must be signed in to change notification settings - Fork 232
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #385 from tiffanyec/python_pcl_update
5.3 Python pcl update
- Loading branch information
Showing
42 changed files
with
1,510 additions
and
335 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
File renamed without changes.
Empty file.
141 changes: 141 additions & 0 deletions
141
exercises/5.3/solution_ws/ros2/src/filter_call/filter_call/filter_call.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,141 @@ | ||
#!/usr/bin/env python | ||
|
||
import rclpy | ||
from rclpy.node import Node | ||
import py_perception.srv | ||
from py_perception.srv import FilterCloud | ||
from sensor_msgs.msg import PointCloud2 | ||
|
||
class FilterNode(Node): | ||
def __init__(self): | ||
super().__init__('filter_cloud', allow_undeclared_parameters = True, | ||
automatically_declare_parameters_from_overrides = True) | ||
self.client = self.create_client(FilterCloud, 'filter_cloud') | ||
while not self.client.wait_for_service(timeout_sec=1.0): | ||
self.get_logger().info('service not available, waiting again...') | ||
|
||
self.pcdfilename = self.get_parameter_or('pcdfilename', rclpy.parameter.Parameter('', rclpy.parameter.Parameter.Type.STRING, '')).value | ||
self.last_cloud = None | ||
|
||
# PUBLISHERS | ||
self.voxel_pub = self.create_publisher(PointCloud2, '/perception_voxelGrid', 1) | ||
self.pass_pub = self.create_publisher(PointCloud2, '/perception_passthrough', 1) | ||
self.plane_pub = self.create_publisher(PointCloud2, '/perception_planeSegmentation', 1) | ||
self.cluster_pub = self.create_publisher(PointCloud2, '/perception_clusterExtraction', 1) | ||
|
||
self.call_filters() | ||
|
||
def call_filters(self): | ||
while rclpy.ok(): | ||
self.voxel_filter() | ||
self.passthrough_filter() | ||
self.plane_segmentation() | ||
self.cluster_extraction() | ||
|
||
def voxel_filter(self): | ||
# ======================= | ||
# VOXEL GRID FILTER | ||
# ======================= | ||
req = FilterCloud.Request() | ||
req.pcdfilename = self.pcdfilename | ||
req.operation = py_perception.srv.FilterCloud.Request.VOXELGRID | ||
|
||
# FROM THE SERVICE, ASSIGN POINTS | ||
req.input_cloud = PointCloud2() | ||
|
||
# ERROR HANDLING | ||
if req.pcdfilename == '': | ||
self.get_logger().error('No file parameter found') | ||
return | ||
|
||
# PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED | ||
future = self.client.call_async(req) | ||
rclpy.spin_until_future_complete(self, future) | ||
res_voxel = future.result() | ||
if not res_voxel.success: | ||
self.get_logger().error('Unsuccessful voxel grid filter operation') | ||
return | ||
|
||
# PUBLISH VOXEL FILTERED POINTCLOUD2 | ||
self.voxel_pub.publish(res_voxel.output_cloud) | ||
self.last_cloud = res_voxel | ||
self.get_logger().info("published: voxel grid filter response") | ||
|
||
def passthrough_filter(self): | ||
# ======================= | ||
# PASSTHROUGH FILTER | ||
# ======================= | ||
req = FilterCloud.Request() | ||
req.pcdfilename = '' | ||
req.operation = py_perception.srv.FilterCloud.Request.PASSTHROUGH | ||
# FROM THE SERVICE, ASSIGN POINTS | ||
req.input_cloud = self.last_cloud.output_cloud | ||
|
||
# PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED | ||
future = self.client.call_async(req) | ||
rclpy.spin_until_future_complete(self, future) | ||
res_pass = future.result() | ||
if not res_pass.success: | ||
self.get_logger().error('Unsuccessful passthrough filter operation') | ||
return | ||
|
||
# PUBLISH PASSTHROUGH FILTERED POINTCLOUD2 | ||
self.pass_pub.publish(res_pass.output_cloud) | ||
self.last_cloud = res_pass | ||
self.get_logger().info("published: passthrough filter response") | ||
|
||
def plane_segmentation(self): | ||
# ======================= | ||
# PLANE SEGMENTATION | ||
# ======================= | ||
req = FilterCloud.Request() | ||
req.pcdfilename = '' | ||
req.operation = py_perception.srv.FilterCloud.Request.PLANESEGMENTATION | ||
# FROM THE SERVICE, ASSIGN POINTS | ||
req.input_cloud = self.last_cloud.output_cloud | ||
|
||
# PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED | ||
future = self.client.call_async(req) | ||
rclpy.spin_until_future_complete(self, future) | ||
res_seg = future.result() | ||
if not res_seg.success: | ||
self.get_logger().error('Unsuccessful plane segmentation operation') | ||
return | ||
|
||
# PUBLISH PLANESEGMENTATION FILTERED POINTCLOUD2 | ||
self.plane_pub.publish(res_seg.output_cloud) | ||
self.last_cloud = res_seg | ||
self.get_logger().info("published: plane segmentation filter response") | ||
|
||
def cluster_extraction(self): | ||
# ======================= | ||
# CLUSTER EXTRACTION | ||
# ======================= | ||
req = FilterCloud.Request() | ||
req.pcdfilename = '' | ||
req.operation = py_perception.srv.FilterCloud.Request.CLUSTEREXTRACTION | ||
# FROM THE SERVICE, ASSIGN POINTS | ||
req.input_cloud = self.last_cloud.output_cloud | ||
|
||
# PACKAGE THE FILTERED POINTCLOUD2 TO BE PUBLISHED | ||
future = self.client.call_async(req) | ||
rclpy.spin_until_future_complete(self, future) | ||
res_cluster = future.result() | ||
if not res_cluster.success: | ||
self.get_logger().error('Unsuccessful cluster extraction operation') | ||
return | ||
|
||
# PUBLISH CLUSTEREXTRACTION FILTERED POINTCLOUD2 | ||
self.cluster_pub.publish(res_cluster.output_cloud) | ||
self.last_cloud = res_cluster | ||
self.get_logger().info("published: cluster extraction filter response") | ||
|
||
|
||
def main(args=None): | ||
rclpy.init(args=args) | ||
node = FilterNode() | ||
rclpy.spin(node) | ||
rclpy.shutdown() | ||
|
||
if __name__ == '__main__': | ||
main() |
24 changes: 24 additions & 0 deletions
24
exercises/5.3/solution_ws/ros2/src/filter_call/package.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,24 @@ | ||
<?xml version="1.0"?> | ||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
<package format="3"> | ||
<name>filter_call</name> | ||
<version>0.0.0</version> | ||
<description>The filter call package</description> | ||
<author email="cgomez@swri.org">cgomez</author> | ||
<author email="tiffany.cappellari@swri.org">tcappellari</author> | ||
<maintainer email="tiffany.cappellari@swri.org">tcappellari</maintainer> | ||
<license>BSD</license> | ||
|
||
<depend>rclpy</depend> | ||
<depend>sensor_msgs</depend> | ||
<depend>py_perception</depend> | ||
|
||
<test_depend>ament_copyright</test_depend> | ||
<test_depend>ament_flake8</test_depend> | ||
<test_depend>ament_pep257</test_depend> | ||
<test_depend>python3-pytest</test_depend> | ||
|
||
<export> | ||
<build_type>ament_python</build_type> | ||
</export> | ||
</package> |
Empty file.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,4 @@ | ||
[develop] | ||
script-dir=$base/lib/filter_call | ||
[install] | ||
install-scripts=$base/lib/filter_call |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,26 @@ | ||
from setuptools import setup | ||
|
||
package_name = 'filter_call' | ||
|
||
setup( | ||
name=package_name, | ||
version='0.0.0', | ||
packages=[package_name], | ||
data_files=[ | ||
('share/ament_index/resource_index/packages', | ||
['resource/' + package_name]), | ||
('share/' + package_name, ['package.xml']), | ||
], | ||
install_requires=['setuptools'], | ||
zip_safe=True, | ||
maintainer='tcappellari', | ||
maintainer_email='tiffany.cappellari@swri.org', | ||
description='The filter call package', | ||
license='BSD', | ||
tests_require=['pytest'], | ||
entry_points={ | ||
'console_scripts': [ | ||
'filter_call = filter_call.filter_call:main' | ||
], | ||
}, | ||
) |
23 changes: 23 additions & 0 deletions
23
exercises/5.3/solution_ws/ros2/src/filter_call/test/test_copyright.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,23 @@ | ||
# Copyright 2015 Open Source Robotics Foundation, Inc. | ||
# | ||
# Licensed under the Apache License, Version 2.0 (the "License"); | ||
# you may not use this file except in compliance with the License. | ||
# You may obtain a copy of the License at | ||
# | ||
# http://www.apache.org/licenses/LICENSE-2.0 | ||
# | ||
# Unless required by applicable law or agreed to in writing, software | ||
# distributed under the License is distributed on an "AS IS" BASIS, | ||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
# See the License for the specific language governing permissions and | ||
# limitations under the License. | ||
|
||
from ament_copyright.main import main | ||
import pytest | ||
|
||
|
||
@pytest.mark.copyright | ||
@pytest.mark.linter | ||
def test_copyright(): | ||
rc = main(argv=['.', 'test']) | ||
assert rc == 0, 'Found errors' |
25 changes: 25 additions & 0 deletions
25
exercises/5.3/solution_ws/ros2/src/filter_call/test/test_flake8.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,25 @@ | ||
# Copyright 2017 Open Source Robotics Foundation, Inc. | ||
# | ||
# Licensed under the Apache License, Version 2.0 (the "License"); | ||
# you may not use this file except in compliance with the License. | ||
# You may obtain a copy of the License at | ||
# | ||
# http://www.apache.org/licenses/LICENSE-2.0 | ||
# | ||
# Unless required by applicable law or agreed to in writing, software | ||
# distributed under the License is distributed on an "AS IS" BASIS, | ||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
# See the License for the specific language governing permissions and | ||
# limitations under the License. | ||
|
||
from ament_flake8.main import main_with_errors | ||
import pytest | ||
|
||
|
||
@pytest.mark.flake8 | ||
@pytest.mark.linter | ||
def test_flake8(): | ||
rc, errors = main_with_errors(argv=[]) | ||
assert rc == 0, \ | ||
'Found %d code style errors / warnings:\n' % len(errors) + \ | ||
'\n'.join(errors) |
23 changes: 23 additions & 0 deletions
23
exercises/5.3/solution_ws/ros2/src/filter_call/test/test_pep257.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,23 @@ | ||
# Copyright 2015 Open Source Robotics Foundation, Inc. | ||
# | ||
# Licensed under the Apache License, Version 2.0 (the "License"); | ||
# you may not use this file except in compliance with the License. | ||
# You may obtain a copy of the License at | ||
# | ||
# http://www.apache.org/licenses/LICENSE-2.0 | ||
# | ||
# Unless required by applicable law or agreed to in writing, software | ||
# distributed under the License is distributed on an "AS IS" BASIS, | ||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
# See the License for the specific language governing permissions and | ||
# limitations under the License. | ||
|
||
from ament_pep257.main import main | ||
import pytest | ||
|
||
|
||
@pytest.mark.linter | ||
@pytest.mark.pep257 | ||
def test_pep257(): | ||
rc = main(argv=['.', 'test']) | ||
assert rc == 0, 'Found code style errors / warnings' |
56 changes: 56 additions & 0 deletions
56
exercises/5.3/solution_ws/ros2/src/py_perception/CMakeLists.txt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,56 @@ | ||
cmake_minimum_required(VERSION 3.5) | ||
project(py_perception) | ||
add_definitions(-std=c++14) | ||
|
||
# Default to C99 | ||
if(NOT CMAKE_C_STANDARD) | ||
set(CMAKE_C_STANDARD 99) | ||
endif() | ||
|
||
# Default to C++14 | ||
if(NOT CMAKE_CXX_STANDARD) | ||
set(CMAKE_CXX_STANDARD 14) | ||
endif() | ||
|
||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") | ||
add_compile_options(-Wall -Wextra -Wpedantic) | ||
endif() | ||
|
||
# find dependencies | ||
find_package(ament_cmake REQUIRED) | ||
find_package(rclcpp REQUIRED) | ||
find_package(sensor_msgs REQUIRED) | ||
find_package(tf2_ros REQUIRED) | ||
find_package(tf2 REQUIRED) | ||
find_package(pcl_ros REQUIRED) | ||
find_package(pcl_conversions REQUIRED) | ||
find_package(tf2_eigen REQUIRED) | ||
find_package(PCL REQUIRED) | ||
find_package(rosidl_default_generators REQUIRED) | ||
|
||
rosidl_generate_interfaces(${PROJECT_NAME} | ||
"srv/FilterCloud.srv" | ||
DEPENDENCIES sensor_msgs | ||
) | ||
|
||
add_executable(perception_node src/py_perception_node.cpp) | ||
ament_target_dependencies(perception_node rclcpp tf2_ros tf2 tf2_eigen sensor_msgs pcl_ros) | ||
rosidl_target_interfaces(perception_node ${PROJECT_NAME} "rosidl_typesupport_cpp") | ||
|
||
install(TARGETS | ||
perception_node | ||
DESTINATION lib/${PROJECT_NAME} | ||
) | ||
|
||
if(BUILD_TESTING) | ||
find_package(ament_lint_auto REQUIRED) | ||
# the following line skips the linter which checks for copyrights | ||
# uncomment the line when a copyright and license is not present in all source files | ||
#set(ament_cmake_copyright_FOUND TRUE) | ||
# the following line skips cpplint (only works in a git repo) | ||
# uncomment the line when this package is not in a git repo | ||
#set(ament_cmake_cpplint_FOUND TRUE) | ||
ament_lint_auto_find_test_dependencies() | ||
endif() | ||
|
||
ament_package() |
31 changes: 31 additions & 0 deletions
31
exercises/5.3/solution_ws/ros2/src/py_perception/package.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,31 @@ | ||
<?xml version="1.0"?> | ||
<package format="3"> | ||
<name>py_perception</name> | ||
<version>0.0.1</version> | ||
<description> | ||
The py_perception package | ||
</description> | ||
<author email="cgomez@swri.org">cgomez</author> | ||
<author email="tiffany.cappellari@swri.org">tcappellari</author> | ||
<maintainer email="tiffany.cappellari@swri.org">tcappellari</maintainer> --> | ||
<license>BSD</license> | ||
|
||
<buildtool_depend>ament_cmake</buildtool_depend> | ||
<buildtool_depend>ament_cmake_python</buildtool_depend> | ||
|
||
<depend>rclcpp</depend> | ||
<depend>rosidl_default_generators</depend> | ||
<depend>sensor_msgs</depend> | ||
<depend>tf2_ros</depend> | ||
<depend>tf2</depend> | ||
<depend>pcl_conversions</depend> | ||
<depend>pcl_ros</depend> | ||
<depend>tf2_eigen</depend> | ||
|
||
<member_of_group>rosidl_interface_packages</member_of_group> | ||
|
||
<export> | ||
<build_type>ament_cmake</build_type> | ||
</export> | ||
|
||
</package> |
Oops, something went wrong.