Migrate extract_indices filter (#394)
* - migrate extract_indices filter - add test to check if filter node/component output result Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * add test_depend to package.xml Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * add launch_testing_ros as test_depend too Signed-off-by: Daisuke Sato <daisukes@cmu.edu> * fixed test not to depend on ros2cli Signed-off-by: Daisuke Sato <daisukes@cmu.edu> --------- Signed-off-by: Daisuke Sato <daisukes@cmu.edu>
This commit is contained in:
Daisuke Sato
GitHub
@@ -0,0 +1,35 @@
|
||||
find_package(ament_cmake_pytest REQUIRED)
|
||||
|
||||
# build dummy publisher node and component
|
||||
add_library(dummy_topics SHARED
|
||||
dummy_topics.cpp
|
||||
)
|
||||
target_link_libraries(dummy_topics ${PCL_LIBRARIES})
|
||||
ament_target_dependencies(dummy_topics
|
||||
rclcpp
|
||||
rclcpp_components
|
||||
pcl_conversions
|
||||
sensor_msgs
|
||||
PCL
|
||||
)
|
||||
|
||||
# generate test ament index to locate dummy_topics comonent from tests
|
||||
set(components "")
|
||||
set(components "${components}pcl_ros_tests_filters::DummyTopics;$<TARGET_FILE:dummy_topics>\n")
|
||||
file(GENERATE
|
||||
OUTPUT
|
||||
"${CMAKE_CURRENT_BINARY_DIR}/test_ament_index/share/ament_index/resource_index/rclcpp_components/pcl_ros_tests_filters"
|
||||
CONTENT "${components}")
|
||||
|
||||
ament_add_pytest_test(test_filter_extract_indices_node_component
|
||||
test_filter_component.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_PLUGIN=pcl_ros::ExtractIndices
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
ament_add_pytest_test(test_filter_extract_indices_node
|
||||
test_filter_executable.py
|
||||
ENV DUMMY_PLUGIN=pcl_ros_tests_filters::DummyTopics
|
||||
FILTER_EXECUTABLE=filter_extract_indices_node
|
||||
APPEND_ENV AMENT_PREFIX_PATH=${CMAKE_CURRENT_BINARY_DIR}/test_ament_index
|
||||
)
|
||||
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* Copyright (c) 2022 Carnegie Mellon University
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the name of Carnegie Mellon University nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||
#include <pcl_msgs/msg/point_indices.hpp>
|
||||
#include <pcl_msgs/msg/model_coefficients.hpp>
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
namespace pcl_ros_tests_filters
|
||||
{
|
||||
class DummyTopics : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
explicit DummyTopics(const rclcpp::NodeOptions & options);
|
||||
|
||||
private:
|
||||
void timer_callback();
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud2_pub_;
|
||||
rclcpp::Publisher<pcl_msgs::msg::PointIndices>::SharedPtr indices_pub_;
|
||||
rclcpp::Publisher<pcl_msgs::msg::ModelCoefficients>::SharedPtr model_pub_;
|
||||
size_t count_;
|
||||
};
|
||||
|
||||
DummyTopics::DummyTopics(const rclcpp::NodeOptions & options)
|
||||
: Node("dummy_point_cloud2_publisher", options), count_(0)
|
||||
{
|
||||
point_cloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("point_cloud2", 10);
|
||||
indices_pub_ = this->create_publisher<pcl_msgs::msg::PointIndices>("indices", 10);
|
||||
model_pub_ = this->create_publisher<pcl_msgs::msg::ModelCoefficients>("model", 10);
|
||||
timer_ = this->create_wall_timer(
|
||||
100ms, std::bind(&DummyTopics::timer_callback, this));
|
||||
}
|
||||
|
||||
void DummyTopics::timer_callback()
|
||||
{
|
||||
builtin_interfaces::msg::Time now_msg;
|
||||
now_msg = get_clock()->now();
|
||||
|
||||
sensor_msgs::msg::PointCloud2 point_cloud2_msg;
|
||||
|
||||
pcl::PointCloud<pcl::PointXYZ> random_pcl;
|
||||
|
||||
unsigned int global_seed = 100;
|
||||
for (int v = 0; v < 1000; ++v) {
|
||||
pcl::PointXYZ newPoint;
|
||||
newPoint.x = (rand_r(&global_seed) * 100.0) / RAND_MAX;
|
||||
newPoint.y = (rand_r(&global_seed) * 100.0) / RAND_MAX;
|
||||
newPoint.z = (rand_r(&global_seed) * 100.0) / RAND_MAX;
|
||||
random_pcl.points.push_back(newPoint);
|
||||
}
|
||||
|
||||
// publish point cloud
|
||||
pcl::toROSMsg<pcl::PointXYZ>(random_pcl, point_cloud2_msg);
|
||||
point_cloud2_msg.header.stamp = now_msg;
|
||||
point_cloud2_pub_->publish(point_cloud2_msg);
|
||||
|
||||
// publish indices
|
||||
pcl_msgs::msg::PointIndices indices_msg;
|
||||
indices_msg.header.stamp = now_msg;
|
||||
indices_msg.indices.push_back(0);
|
||||
indices_pub_->publish(indices_msg);
|
||||
|
||||
// publish model
|
||||
pcl_msgs::msg::ModelCoefficients model_msg;
|
||||
model_msg.header.stamp = now_msg;
|
||||
model_msg.values.push_back(0);
|
||||
model_msg.values.push_back(0);
|
||||
model_msg.values.push_back(1);
|
||||
model_msg.values.push_back(0);
|
||||
model_pub_->publish(model_msg);
|
||||
}
|
||||
|
||||
} // namespace pcl_ros_tests_filters
|
||||
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(pcl_ros_tests_filters::DummyTopics)
|
||||
@@ -0,0 +1,90 @@
|
||||
#
|
||||
# Copyright (c) 2022 Carnegie Mellon University
|
||||
# All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions are met:
|
||||
#
|
||||
# * Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# * Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in the
|
||||
# documentation and/or other materials provided with the distribution.
|
||||
# * Neither the name of Carnegie Mellon University nor the names of its
|
||||
# contributors may be used to endorse or promote products derived from
|
||||
# this software without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
|
||||
import os
|
||||
import unittest
|
||||
|
||||
import launch
|
||||
import launch.actions
|
||||
import launch_ros.actions
|
||||
import launch_testing.actions
|
||||
import launch_testing.markers
|
||||
import pytest
|
||||
|
||||
from launch_testing_ros import WaitForTopics
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
|
||||
|
||||
@pytest.mark.launch_test
|
||||
@launch_testing.markers.keep_alive
|
||||
def generate_test_description():
|
||||
dummy_plugin = os.getenv('DUMMY_PLUGIN')
|
||||
filter_plugin = os.getenv('FILTER_PLUGIN')
|
||||
use_indices = os.getenv('USE_INDICES')
|
||||
approximate_sync = os.getenv('APPROXIMATE_SYNC')
|
||||
|
||||
parameters = {}
|
||||
if use_indices:
|
||||
parameters['use_indices'] = (use_indices == 'true')
|
||||
if approximate_sync:
|
||||
parameters['approximate_sync'] = (approximate_sync == 'true')
|
||||
print(parameters)
|
||||
|
||||
return launch.LaunchDescription([
|
||||
launch_ros.actions.ComposableNodeContainer(
|
||||
name='filter_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container',
|
||||
composable_node_descriptions=[
|
||||
launch_ros.descriptions.ComposableNode(
|
||||
package='pcl_ros_tests_filters',
|
||||
plugin=dummy_plugin,
|
||||
name='dummy_publisher',
|
||||
),
|
||||
launch_ros.descriptions.ComposableNode(
|
||||
package='pcl_ros',
|
||||
plugin=filter_plugin,
|
||||
name='filter_node',
|
||||
remappings=[('/input', '/point_cloud2')],
|
||||
parameters=[parameters],
|
||||
),
|
||||
],
|
||||
output='screen',
|
||||
),
|
||||
launch_testing.actions.ReadyToTest()
|
||||
])
|
||||
|
||||
|
||||
class TestFilter(unittest.TestCase):
|
||||
def test_filter_output(self):
|
||||
wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0)
|
||||
assert wait_for_topics.wait()
|
||||
assert 'output' in wait_for_topics.topics_received(), "Didn't receive message"
|
||||
wait_for_topics.shutdown()
|
||||
@@ -0,0 +1,60 @@
|
||||
import os
|
||||
import unittest
|
||||
|
||||
import launch
|
||||
import launch.actions
|
||||
import launch_ros.actions
|
||||
import launch_testing.actions
|
||||
import launch_testing.markers
|
||||
import pytest
|
||||
|
||||
from launch_testing_ros import WaitForTopics
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
|
||||
|
||||
@pytest.mark.launch_test
|
||||
@launch_testing.markers.keep_alive
|
||||
def generate_test_description():
|
||||
dummy_plugin = os.getenv('DUMMY_PLUGIN')
|
||||
filter_executable = os.getenv('FILTER_EXECUTABLE')
|
||||
use_indices = os.getenv('USE_INDICES')
|
||||
approximate_sync = os.getenv('APPROXIMATE_SYNC')
|
||||
|
||||
ros_arguments = ["-r", "input:=point_cloud2"]
|
||||
if use_indices:
|
||||
ros_arguments.extend(["-p", "use_indices:={}".format(use_indices)])
|
||||
if approximate_sync:
|
||||
ros_arguments.extend(["-p", "approximate_sync:={}".format(approximate_sync)])
|
||||
|
||||
return launch.LaunchDescription([
|
||||
|
||||
launch_ros.actions.ComposableNodeContainer(
|
||||
name='filter_container',
|
||||
namespace='',
|
||||
package='rclcpp_components',
|
||||
executable='component_container',
|
||||
composable_node_descriptions=[
|
||||
launch_ros.descriptions.ComposableNode(
|
||||
package='pcl_ros_tests_filters',
|
||||
plugin=dummy_plugin,
|
||||
name='dummy_publisher',
|
||||
),
|
||||
],
|
||||
output='screen',
|
||||
),
|
||||
launch_ros.actions.Node(
|
||||
package='pcl_ros',
|
||||
executable=filter_executable,
|
||||
output="screen",
|
||||
ros_arguments=ros_arguments
|
||||
),
|
||||
launch_testing.actions.ReadyToTest()
|
||||
])
|
||||
|
||||
|
||||
class TestFilter(unittest.TestCase):
|
||||
def test_filter_output(self):
|
||||
wait_for_topics = WaitForTopics([('output', PointCloud2)], timeout=5.0)
|
||||
assert wait_for_topics.wait()
|
||||
assert 'output' in wait_for_topics.topics_received(), "Didn't receive message"
|
||||
wait_for_topics.shutdown()
|
||||
Reference in New Issue
Block a user