houyongdi/dreamDeckAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/velodyne/velodyne_laserscan/CHANGELOG.rst
+42
View File
@@ -0,0 +1,42 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package velodyne_laserscan
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.5.2 (2019-01-28)
------------------
1.5.1 (2018-12-10)
------------------
1.5.0 (2018-10-19)
------------------
1.4.0 (2018-09-19)
------------------
* Merge pull request `#170 <https://github.com/ros-drivers/velodyne/issues/170>`_ from ros-drivers/maint/move_header_files
* Moving header files to traditional location inside include folders.
* Merge pull request `#160 <https://github.com/ros-drivers/velodyne/issues/160>`_ from ros-drivers/maint/updating_package_xml_to_v2
* Updated all package.xmls to ver 2. Cleaned up catkin_lint errors.
All package.xml files are now compatible with version 2 of the
package.xml specification in REP 140. Removed some unnecessary
execute permissions on a few files. Fixed a missing test_depend.
* Merge pull request `#146 <https://github.com/ros-drivers/velodyne/issues/146>`_ from stsundermann/patch-2
Use std::abs instead of fabsf
* Merge pull request `#150 <https://github.com/ros-drivers/velodyne/issues/150>`_ from ros-drivers/mikaelarguedas-patch-1
* update to use non deprecated pluginlib macro
* Use std::abs instead of fabsf
cfg\_.resolution is double but fabsf takes a float which may cause truncation of value.
* Contributors: Andre Volk, CNR, Joshua Whitley, Mikael Arguedas, Stephan Sundermann
1.3.0 (2017-11-10)
------------------
* Merge pull request `#110 <https://github.com/ros-drivers/velodyne/issues/110>`_ from kmhallen/master
Added velodyne_laserscan package
* Added tests for velodyne_laserscan
* Fixed validating PointCloud2 field types
* Package.xml format version 2
* Merge pull request `#1 <https://github.com/ros-drivers/velodyne/issues/1>`_ from volkandre/master
Fixed bug. Laserscans now cover full 360 degrees.
* Fixed bug. Laserscans now cover full 360 degrees.
* Added velodyne_laserscan package and inserted into existing launch files
* Contributors: Joshua Whitley, Kevin Hallenbeck, kmhallen, volkandre
sensors/velodyne/velodyne_laserscan/CMakeLists.txt
+66
View File
@@ -0,0 +1,66 @@
cmake_minimum_required(VERSION 2.8.3)
project(velodyne_laserscan)
# Set minimum C++ standard to C++11
if (NOT "${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}")
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif ("${CMAKE_CXX_STANDARD_COMPUTED_DEFAULT}" STREQUAL "98")
message(STATUS "Changing CXX_STANDARD from C++98 to C++11")
set(CMAKE_CXX_STANDARD 11)
endif()
find_package(catkin REQUIRED COMPONENTS
roscpp
nodelet
sensor_msgs
dynamic_reconfigure
roslint
)
generate_dynamic_reconfigure_options(
cfg/VelodyneLaserScan.cfg
)
catkin_package(CATKIN_DEPENDS
sensor_msgs
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
src/velodyne_laserscan.cpp
src/nodelet.cpp
)
add_dependencies(${PROJECT_NAME}
${PROJECT_NAME}_gencfg
)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
)
add_executable(${PROJECT_NAME}_node
src/node.cpp
)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
${PROJECT_NAME}
)
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(FILES nodelets.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
roslint_cpp()
if (CATKIN_ENABLE_TESTING)
add_subdirectory(tests)
endif()
sensors/velodyne/velodyne_laserscan/cfg/VelodyneLaserScan.cfg
+13
View File
@@ -0,0 +1,13 @@
#! /usr/bin/env python
PACKAGE='velodyne_laserscan'
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Lvl Description Default Min Max
gen.add("ring", int_t, 0, "Ring to extract as laser scan (-1 default)", -1, -1, 31)
gen.add("resolution", double_t, 0, "Laser scan angular resolution (rad)", 0.007, 0.001, 0.05)
exit(gen.generate(PACKAGE, PACKAGE, "VelodyneLaserScan"))
sensors/velodyne/velodyne_laserscan/include/velodyne_laserscan/velodyne_laserscan.h
+72
View File
@@ -0,0 +1,72 @@
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// 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 {copyright_holder} 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.
#ifndef VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H
#define VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <boost/thread/mutex.hpp>
#include <boost/thread/lock_guard.hpp>
#include <dynamic_reconfigure/server.h>
#include <velodyne_laserscan/VelodyneLaserScanConfig.h>
namespace velodyne_laserscan
{
class VelodyneLaserScan
{
public:
VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv);
private:
boost::mutex connect_mutex_;
void connectCb();
void recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg);
ros::NodeHandle nh_;
ros::Subscriber sub_;
ros::Publisher pub_;
VelodyneLaserScanConfig cfg_;
dynamic_reconfigure::Server<VelodyneLaserScanConfig> srv_;
void reconfig(VelodyneLaserScanConfig& config, uint32_t level);
unsigned int ring_count_;
};
} // namespace velodyne_laserscan
#endif // VELODYNE_LASERSCAN_VELODYNE_LASERSCAN_H
sensors/velodyne/velodyne_laserscan/nodelets.xml
+10
View File
@@ -0,0 +1,10 @@
<library path="libvelodyne_laserscan">
<class name="velodyne_laserscan/LaserScanNodelet"
type="velodyne_laserscan::LaserScanNodelet"
base_class_type="nodelet::Nodelet">
<description>
Extract a single ring from a Velodyne PointCloud2 and publish
as a LaserScan.
</description>
</class>
</library>
sensors/velodyne/velodyne_laserscan/package.xml
+32
View File
@@ -0,0 +1,32 @@
<?xml version="1.0"?>
<package format="2">
<name>velodyne_laserscan</name>
<version>1.5.2</version>
<description>
Extract a single ring of a Velodyne PointCloud2 and publish it as a LaserScan message
</description>
<maintainer email="josh.whitley@autoware.org">Josh Whitley</maintainer>
<author>Micho Radovnikovich</author>
<author>Kevin Hallenbeck</author>
<license>BSD</license>
<url type="website">http://ros.org/wiki/velodyne_laserscan</url>
<url type="repository">https://github.com/ros-drivers/velodyne</url>
<url type="bugtracker">https://github.com/ros-drivers/velodyne/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roslint</build_depend>
<depend>roscpp</depend>
<depend>nodelet</depend>
<depend>sensor_msgs</depend>
<depend>dynamic_reconfigure</depend>
<test_depend>roslaunch</test_depend>
<test_depend>rostest</test_depend>
<export>
<nodelet plugin="${prefix}/nodelets.xml" />
</export>
</package>
sensors/velodyne/velodyne_laserscan/src/node.cpp
+49
View File
@@ -0,0 +1,49 @@
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// 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 {copyright_holder} 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 <ros/ros.h>
#include "velodyne_laserscan/velodyne_laserscan.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "velodyne_laserscan_node");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
// create VelodyneLaserScan class
velodyne_laserscan::VelodyneLaserScan n(nh, nh_priv);
// handle callbacks until shut down
ros::spin();
return 0;
}
sensors/velodyne/velodyne_laserscan/src/nodelet.cpp
+58
View File
@@ -0,0 +1,58 @@
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// 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 {copyright_holder} 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 <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include "velodyne_laserscan/velodyne_laserscan.h"
namespace velodyne_laserscan
{
class LaserScanNodelet: public nodelet::Nodelet
{
public:
LaserScanNodelet() {}
~LaserScanNodelet() {}
private:
virtual void onInit()
{
node_.reset(new VelodyneLaserScan(getNodeHandle(), getPrivateNodeHandle()));
}
boost::shared_ptr<VelodyneLaserScan> node_;
};
} // namespace velodyne_laserscan
PLUGINLIB_EXPORT_CLASS(velodyne_laserscan::LaserScanNodelet, nodelet::Nodelet);
sensors/velodyne/velodyne_laserscan/src/velodyne_laserscan.cpp
+280
View File
@@ -0,0 +1,280 @@
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// 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 {copyright_holder} 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 "velodyne_laserscan/velodyne_laserscan.h"
#include <sensor_msgs/point_cloud2_iterator.h>
namespace velodyne_laserscan
{
VelodyneLaserScan::VelodyneLaserScan(ros::NodeHandle &nh, ros::NodeHandle &nh_priv) :
nh_(nh), srv_(nh_priv), ring_count_(0)
{
ros::SubscriberStatusCallback connect_cb = boost::bind(&VelodyneLaserScan::connectCb, this);
pub_ = nh.advertise<sensor_msgs::LaserScan>("scan", 10, connect_cb, connect_cb);
srv_.setCallback(boost::bind(&VelodyneLaserScan::reconfig, this, _1, _2));
}
void VelodyneLaserScan::connectCb()
{
boost::lock_guard<boost::mutex> lock(connect_mutex_);
if (!pub_.getNumSubscribers())
{
sub_.shutdown();
}
else if (!sub_)
{
sub_ = nh_.subscribe("velodyne_points", 10, &VelodyneLaserScan::recvCallback, this);
}
}
void VelodyneLaserScan::recvCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
// Latch ring count
if (!ring_count_)
{
// Check for PointCloud2 field 'ring'
bool found = false;
for (size_t i = 0; i < msg->fields.size(); i++)
{
if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
{
if (msg->fields[i].name == "ring")
{
found = true;
break;
}
}
}
if (!found)
{
ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
return;
}
for (sensor_msgs::PointCloud2ConstIterator<uint16_t> it(*msg, "ring"); it != it.end(); ++it)
{
const uint16_t ring = *it;
if (ring + 1 > ring_count_)
{
ring_count_ = ring + 1;
}
}
if (ring_count_)
{
ROS_INFO("VelodyneLaserScan: Latched ring count of %u", ring_count_);
}
else
{
ROS_ERROR("VelodyneLaserScan: Field 'ring' of type 'UINT16' not present in PointCloud2");
return;
}
}
// Select ring to use
uint16_t ring;
if ((cfg_.ring < 0) || (cfg_.ring >= ring_count_))
{
// Default to ring closest to being level for each known sensor
if (ring_count_ > 32)
{
ring = 57; // HDL-64E
}
else if (ring_count_ > 16)
{
ring = 23; // HDL-32E
}
else
{
ring = 8; // VLP-16
}
}
else
{
ring = cfg_.ring;
}
ROS_INFO_ONCE("VelodyneLaserScan: Extracting ring %u", ring);
// Load structure of PointCloud2
int offset_x = -1;
int offset_y = -1;
int offset_z = -1;
int offset_i = -1;
int offset_r = -1;
for (size_t i = 0; i < msg->fields.size(); i++)
{
if (msg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
{
if (msg->fields[i].name == "x")
{
offset_x = msg->fields[i].offset;
}
else if (msg->fields[i].name == "y")
{
offset_y = msg->fields[i].offset;
}
else if (msg->fields[i].name == "z")
{
offset_z = msg->fields[i].offset;
}
else if (msg->fields[i].name == "intensity")
{
offset_i = msg->fields[i].offset;
}
}
else if (msg->fields[i].datatype == sensor_msgs::PointField::UINT16)
{
if (msg->fields[i].name == "ring")
{
offset_r = msg->fields[i].offset;
}
}
}
// Construct LaserScan message
if ((offset_x >= 0) && (offset_y >= 0) && (offset_r >= 0))
{
const float RESOLUTION = std::abs(cfg_.resolution);
const size_t SIZE = 2.0 * M_PI / RESOLUTION;
sensor_msgs::LaserScanPtr scan(new sensor_msgs::LaserScan());
scan->header = msg->header;
scan->angle_increment = RESOLUTION;
scan->angle_min = -M_PI;
scan->angle_max = M_PI;
scan->range_min = 0.0;
scan->range_max = 200.0;
scan->time_increment = 0.0;
scan->ranges.resize(SIZE, INFINITY);
if ((offset_x == 0) &&
(offset_y == 4) &&
(offset_i % 4 == 0) &&
(offset_r % 4 == 0))
{
scan->intensities.resize(SIZE);
const size_t X = 0;
const size_t Y = 1;
const size_t I = offset_i / 4;
const size_t R = offset_r / 4;
for (sensor_msgs::PointCloud2ConstIterator<float> it(*msg, "x"); it != it.end(); ++it)
{
const uint16_t r = *((const uint16_t*)(&it[R])); // ring
if (r == ring)
{
const float x = it[X]; // x
const float y = it[Y]; // y
const float i = it[I]; // intensity
const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
{
scan->ranges[bin] = sqrtf(x * x + y * y);
scan->intensities[bin] = i;
}
}
}
}
else
{
ROS_WARN_ONCE("VelodyneLaserScan: PointCloud2 fields in unexpected order. Using slower generic method.");
if (offset_i >= 0)
{
scan->intensities.resize(SIZE);
sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
sensor_msgs::PointCloud2ConstIterator<float> iter_i(*msg, "intensity");
for ( ; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r, ++iter_i)
{
const uint16_t r = *iter_r; // ring
if (r == ring)
{
const float x = *iter_x; // x
const float y = *iter_y; // y
const float i = *iter_i; // intensity
const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
{
scan->ranges[bin] = sqrtf(x * x + y * y);
scan->intensities[bin] = i;
}
}
}
}
else
{
sensor_msgs::PointCloud2ConstIterator<uint16_t> iter_r(*msg, "ring");
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
for (; iter_r != iter_r.end(); ++iter_x, ++iter_y, ++iter_r)
{
const uint16_t r = *iter_r; // ring
if (r == ring)
{
const float x = *iter_x; // x
const float y = *iter_y; // y
const int bin = (atan2f(y, x) + static_cast<float>(M_PI)) / RESOLUTION;
if ((bin >= 0) && (bin < static_cast<int>(SIZE)))
{
scan->ranges[bin] = sqrtf(x * x + y * y);
}
}
}
}
}
pub_.publish(scan);
}
else
{
ROS_ERROR("VelodyneLaserScan: PointCloud2 missing one or more required fields! (x,y,ring)");
}
}
void VelodyneLaserScan::reconfig(VelodyneLaserScanConfig& config, uint32_t level)
{
cfg_ = config;
}
} // namespace velodyne_laserscan
sensors/velodyne/velodyne_laserscan/tests/CMakeLists.txt
+30
View File
@@ -0,0 +1,30 @@
### Unit tests
#
# Only configured when CATKIN_ENABLE_TESTING is true.
# These dependencies are only needed for unit testing
find_package(roslaunch REQUIRED)
find_package(rostest REQUIRED)
# C++ gtests
#catkin_add_gtest(test_calibration test_calibration.cpp)
#add_dependencies(test_calibration ${catkin_EXPORTED_TARGETS})
#target_link_libraries(test_calibration velodyne_rawdata ${catkin_LIBRARIES})
# ROS rostests
add_rostest_gtest(test_lazy_subscriber_node lazy_subscriber_node.test lazy_subscriber.cpp)
add_dependencies(test_lazy_subscriber_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_lazy_subscriber_node ${catkin_LIBRARIES})
add_rostest_gtest(test_lazy_subscriber_nodelet lazy_subscriber_nodelet.test lazy_subscriber.cpp)
add_dependencies(test_lazy_subscriber_nodelet ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_lazy_subscriber_nodelet ${catkin_LIBRARIES})
add_rostest_gtest(test_system_node system_node.test system.cpp)
add_dependencies(test_system_node ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_system_node ${catkin_LIBRARIES})
add_rostest_gtest(test_system_nodelet system_nodelet.test system.cpp)
add_dependencies(test_system_nodelet ${catkin_EXPORTED_TARGETS})
target_link_libraries(test_system_nodelet ${catkin_LIBRARIES})
sensors/velodyne/velodyne_laserscan/tests/lazy_subscriber.cpp
+124
View File
@@ -0,0 +1,124 @@
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// 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 {copyright_holder} 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 <gtest/gtest.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
// Subscriber receive callback
void recv(const sensor_msgs::LaserScanConstPtr& msg) {}
// Build and publish a minimal PointCloud2 message
void publish(const ros::Publisher &pub)
{
const uint32_t POINT_STEP = 32;
sensor_msgs::PointCloud2 msg;
msg.header.frame_id = "";
msg.header.stamp = ros::Time::now();
msg.fields.resize(5);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.fields[3].name = "intensity";
msg.fields[3].offset = 16;
msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[3].count = 1;
msg.fields[4].name = "ring";
msg.fields[4].offset = 20;
msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
msg.fields[4].count = 1;
msg.data.resize(1 * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
msg.is_bigendian = false;
msg.is_dense = true;
pub.publish(msg);
}
// Verify correct handling of subscribe and unsubscribe events
TEST(Main, subscribe_unsubscribe)
{
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<sensor_msgs::PointCloud2>("velodyne_points", 2);
// Wait for node to startup
ros::WallDuration(2.0).sleep();
ros::spinOnce();
EXPECT_EQ(0, pub.getNumSubscribers());
// Subscribe to 'scan' and expect the node to subscribe to 'velodyne_points'
ros::Subscriber sub = nh.subscribe("scan", 2, recv);
for (size_t i = 10; i > 0; i--)
{
publish(pub);
ros::WallDuration(0.1).sleep();
ros::spinOnce();
}
EXPECT_EQ(1, sub.getNumPublishers());
EXPECT_EQ(1, pub.getNumSubscribers());
// Unsubscribe from 'scan' and expect the node to unsubscribe from 'velodyne_points'
sub.shutdown();
for (size_t i = 10; i > 0; i--)
{
publish(pub);
ros::WallDuration(0.1).sleep();
ros::spinOnce();
}
EXPECT_EQ(0, sub.getNumPublishers());
EXPECT_EQ(0, pub.getNumSubscribers());
}
// Run all the tests that were declared with TEST()
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "test_lazy_subscriber");
return RUN_ALL_TESTS();
}
sensors/velodyne/velodyne_laserscan/tests/lazy_subscriber_node.test
+17
View File
@@ -0,0 +1,17 @@
<!-- -*- mode: XML -*- -->
<!-- rostest of the lazy subscriber -->
<launch>
<!-- Select log or screen output -->
<arg name="output" default="log"/> <!-- screen/log -->
<!-- Start the laserscan node -->
<node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
<!-- Start the rostest -->
<test test-name="test_lazy_subscriber_node" pkg="velodyne_laserscan"
type="test_lazy_subscriber_node" name="test_lazy_subscriber">
</test>
</launch>
sensors/velodyne/velodyne_laserscan/tests/lazy_subscriber_nodelet.test
+21
View File
@@ -0,0 +1,21 @@
<!-- -*- mode: XML -*- -->
<!-- rostest of the lazy subscriber -->
<launch>
<!-- Select log or screen output -->
<arg name="output" default="log"/> <!-- screen/log -->
<!-- Start the laserscan nodelet -->
<arg name="manager" default="nodelet_manager" />
<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="$(arg output)" />
<node pkg="nodelet" type="nodelet" name="velodyne_laserscan" output="$(arg output)"
args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
</node>
<!-- Start the rostest -->
<test test-name="test_lazy_subscriber_nodelet" pkg="velodyne_laserscan"
type="test_lazy_subscriber_nodelet" name="test_lazy_subscriber">
</test>
</launch>
sensors/velodyne/velodyne_laserscan/tests/system.cpp
+711
View File
@@ -0,0 +1,711 @@
// Copyright (C) 2018, 2019 Kevin Hallenbeck, Joshua Whitley
// All rights reserved.
//
// Software License Agreement (BSD License 2.0)
//
// 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 {copyright_holder} 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 <gtest/gtest.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <cstdlib>
#include <algorithm>
#include <vector>
// Define our own PointCloud type for easy use
typedef struct
{
float x; // x
float y; // y
float z; // z
float i; // intensity
uint16_t r; // ring
}
Point;
typedef struct
{
std_msgs::Header header;
std::vector<Point> points;
}
PointCloud;
// Global variables
ros::Publisher g_pub;
ros::Subscriber g_sub;
sensor_msgs::LaserScan g_scan;
volatile bool g_scan_new = false;
// Convert WallTime to Time
static inline ros::Time rosTime(const ros::WallTime &stamp)
{
return ros::Time(stamp.sec, stamp.nsec);
}
// Subscriber receive callback
void recv(const sensor_msgs::LaserScanConstPtr& msg)
{
g_scan = *msg;
g_scan_new = true;
}
// Wait for incoming LaserScan message
bool waitForScan(ros::WallDuration dur)
{
const ros::WallTime start = ros::WallTime::now();
while (!g_scan_new)
{
if ((ros::WallTime::now() - start) > dur)
{
return false;
}
ros::WallDuration(0.001).sleep();
ros::spinOnce();
}
return true;
}
// Build and publish PointCloud2 messages of various structures
void publishXYZIR1(const PointCloud &cloud)
{
g_scan_new = false;
const uint32_t POINT_STEP = 32;
sensor_msgs::PointCloud2 msg;
msg.header.frame_id = cloud.header.frame_id;
msg.header.stamp = cloud.header.stamp;
msg.fields.resize(5);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.fields[3].name = "intensity";
msg.fields[3].offset = 16;
msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[3].count = 1;
msg.fields[4].name = "ring";
msg.fields[4].offset = 20;
msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
msg.fields[4].count = 1;
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
msg.is_bigendian = false;
msg.is_dense = true;
uint8_t *ptr = msg.data.data();
for (size_t i = 0; i < cloud.points.size(); i++)
{
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
*(reinterpret_cast<float*>(ptr + 16)) = cloud.points[i].i;
*(reinterpret_cast<uint16_t*>(ptr + 20)) = cloud.points[i].r;
ptr += POINT_STEP;
}
g_pub.publish(msg);
}
void publishXYZIR2(const PointCloud &cloud)
{
g_scan_new = false;
const uint32_t POINT_STEP = 19;
sensor_msgs::PointCloud2 msg;
msg.header.frame_id = cloud.header.frame_id;
msg.header.stamp = cloud.header.stamp;
msg.fields.resize(5);
msg.fields[0].name = "z";
msg.fields[0].offset = 4;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 8;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "x";
msg.fields[2].offset = 12;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.fields[3].name = "intensity";
msg.fields[3].offset = 0;
msg.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[3].count = 1;
msg.fields[4].name = "ring";
msg.fields[4].offset = 16;
msg.fields[4].datatype = sensor_msgs::PointField::UINT16;
msg.fields[4].count = 1;
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
msg.is_bigendian = false;
msg.is_dense = true;
uint8_t *ptr = msg.data.data();
for (size_t i = 0; i < cloud.points.size(); i++)
{
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].i;
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].z;
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].y;
*(reinterpret_cast<float*>(ptr + 12)) = cloud.points[i].x;
*(reinterpret_cast<uint16_t*>(ptr + 16)) = cloud.points[i].r;
ptr += POINT_STEP;
}
g_pub.publish(msg);
}
void publishXYZR(const PointCloud &cloud)
{
g_scan_new = false;
const uint32_t POINT_STEP = 15;
sensor_msgs::PointCloud2 msg;
msg.header.frame_id = cloud.header.frame_id;
msg.header.stamp = cloud.header.stamp;
msg.fields.resize(4);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.fields[3].name = "ring";
msg.fields[3].offset = 12;
msg.fields[3].datatype = sensor_msgs::PointField::UINT16;
msg.fields[3].count = 1;
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
msg.is_bigendian = false;
msg.is_dense = true;
uint8_t *ptr = msg.data.data();
for (size_t i = 0; i < cloud.points.size(); i++)
{
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
*(reinterpret_cast<uint16_t*>(ptr + 12)) = cloud.points[i].r;
ptr += POINT_STEP;
}
g_pub.publish(msg);
}
void publishR(const PointCloud &cloud)
{
g_scan_new = false;
const uint32_t POINT_STEP = 2;
sensor_msgs::PointCloud2 msg;
msg.header.stamp = rosTime(ros::WallTime::now());
msg.fields.resize(1);
msg.fields[0].name = "ring";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::UINT16;
msg.fields[0].count = 1;
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
uint8_t *ptr = msg.data.data();
for (size_t i = 0; i < cloud.points.size(); i++)
{
*(reinterpret_cast<uint16_t*>(ptr + 0)) = cloud.points[i].r;
ptr += POINT_STEP;
}
g_pub.publish(msg);
}
void publishXYZR32(const PointCloud &cloud)
{
g_scan_new = false;
const uint32_t POINT_STEP = 16;
sensor_msgs::PointCloud2 msg;
msg.header.frame_id = cloud.header.frame_id;
msg.header.stamp = cloud.header.stamp;
msg.fields.resize(4);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.fields[3].name = "ring";
msg.fields[3].offset = 12;
msg.fields[3].datatype = sensor_msgs::PointField::UINT32;
msg.fields[3].count = 1;
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
msg.is_bigendian = false;
msg.is_dense = true;
uint8_t *ptr = msg.data.data();
for (size_t i = 0; i < cloud.points.size(); i++)
{
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
*(reinterpret_cast<uint32_t*>(ptr + 12)) = cloud.points[i].r;
ptr += POINT_STEP;
}
g_pub.publish(msg);
}
void publishXYZ(const PointCloud &cloud)
{
g_scan_new = false;
const uint32_t POINT_STEP = 12;
sensor_msgs::PointCloud2 msg;
msg.header.stamp = rosTime(ros::WallTime::now());
msg.fields.resize(3);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 4;
msg.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 8;
msg.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
msg.fields[2].count = 1;
msg.data.resize(std::max((size_t)1, cloud.points.size()) * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
uint8_t *ptr = msg.data.data();
for (size_t i = 0; i < cloud.points.size(); i++)
{
*(reinterpret_cast<float*>(ptr + 0)) = cloud.points[i].x;
*(reinterpret_cast<float*>(ptr + 4)) = cloud.points[i].y;
*(reinterpret_cast<float*>(ptr + 8)) = cloud.points[i].z;
ptr += POINT_STEP;
}
g_pub.publish(msg);
}
void publishNone()
{
g_scan_new = false;
const uint32_t POINT_STEP = 16;
sensor_msgs::PointCloud2 msg;
msg.header.stamp = rosTime(ros::WallTime::now());
msg.data.resize(1 * POINT_STEP, 0x00);
msg.point_step = POINT_STEP;
msg.row_step = msg.data.size();
msg.height = 1;
msg.width = msg.row_step / POINT_STEP;
g_pub.publish(msg);
}
// Find the index of the point in the PointCloud with the shortest 2d distance to the point (x,y)
static inline float SQUARE(float x)
{
return x * x;
}
size_t findClosestIndex(const PointCloud &cloud, uint16_t ring, float x, float y)
{
size_t index = SIZE_MAX;
float delta = INFINITY;
for (size_t i = 0; i < cloud.points.size(); i++)
{
if (cloud.points[i].r == ring)
{
float dist = SQUARE(x - cloud.points[i].x) + SQUARE(y - cloud.points[i].y);
if (dist < delta)
{
delta = dist;
index = i;
}
}
}
return index;
}
// Verify that all LaserScan header values are values are passed through, and other values are default
void verifyScanEmpty(const PointCloud &cloud, bool intensity = true)
{
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
for (size_t i = 0; i < g_scan.ranges.size(); i++)
{
EXPECT_EQ(INFINITY, g_scan.ranges[i]);
}
if (!intensity)
{
EXPECT_EQ(0, g_scan.intensities.size());
}
else
{
EXPECT_EQ(g_scan.ranges.size(), g_scan.intensities.size());
for (size_t i = 0; i < g_scan.intensities.size(); i++)
{
EXPECT_EQ(0.0, g_scan.intensities[i]);
}
}
}
// Verify that every PointCloud point made it to the LaserScan and other values are default
void verifyScanSparse(const PointCloud &cloud, uint16_t ring, uint16_t ring_count, bool intensity = true)
{
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size());
size_t count = 0;
for (size_t i = 0; i < g_scan.ranges.size(); i++)
{
double r = g_scan.ranges[i];
if (std::isfinite(r))
{
float a = g_scan.angle_min + i * g_scan.angle_increment;
float x = g_scan.ranges[i] * cosf(a);
float y = g_scan.ranges[i] * sinf(a);
float e = g_scan.ranges[i] * g_scan.angle_increment + static_cast<float>(1e-3); // allowable error
size_t index = findClosestIndex(cloud, ring, x, y);
if (index < cloud.points.size())
{
count++;
EXPECT_NEAR(cloud.points[index].x, x, e);
EXPECT_NEAR(cloud.points[index].y, y, e);
if (i < g_scan.intensities.size())
{
EXPECT_EQ(cloud.points[index].i, g_scan.intensities[i]);
}
}
else
{
EXPECT_TRUE(false); // LaserScan point not found in PointCloud
}
}
else
{
EXPECT_EQ(INFINITY, r);
}
}
if (ring_count > 0)
{
EXPECT_EQ(cloud.points.size() / ring_count, count); // Make sure that all points were converted to ranges
}
}
// Verify that every LaserScan point is not default, and every point came from the PointCloud
void verifyScanDense(const PointCloud &cloud, uint16_t ring, bool intensity = true)
{
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
EXPECT_EQ(cloud.header.frame_id, g_scan.header.frame_id);
EXPECT_EQ(intensity ? g_scan.ranges.size() : 0, g_scan.intensities.size());
for (size_t i = 0; i < g_scan.ranges.size(); i++)
{
double r = g_scan.ranges[i];
if (std::isfinite(r))
{
float a = g_scan.angle_min + i * g_scan.angle_increment;
float x = g_scan.ranges[i] * cosf(a);
float y = g_scan.ranges[i] * sinf(a);
float e = g_scan.ranges[i] * g_scan.angle_increment + static_cast<float>(1e-3); // allowable error
size_t index = findClosestIndex(cloud, ring, x, y);
if (index < cloud.points.size())
{
EXPECT_NEAR(cloud.points[index].x, x, e);
EXPECT_NEAR(cloud.points[index].y, y, e);
// @TODO: Test for matching intensity
}
else
{
EXPECT_TRUE(false); // LaserScan point not found in PointCloud
}
}
else
{
EXPECT_TRUE(false); // Dense PointCloud should populate every range in LaserScan
}
}
}
// Verify that no LaserScan is generated when the PointCloud2 message is missing required fields
TEST(System, missing_fields)
{
// Make sure system is connected
ASSERT_EQ(1, g_sub.getNumPublishers());
ASSERT_EQ(1, g_pub.getNumSubscribers());
// Create PointCloud with 16 rings
PointCloud cloud;
cloud.points.resize(1);
cloud.points[0].x = 0.0;
cloud.points[0].y = 0.0;
cloud.points[0].z = 0.0;
cloud.points[0].i = 0.0;
cloud.points[0].r = 15;
// Verify no LaserScan when PointCloud2 fields are empty
publishNone();
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
// Verify no LaserScan when PointCloud2 fields are missing 'ring'
publishXYZ(cloud);
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
// Verify no LaserScan when PointCloud2 field 'ring' is the incorrect type
publishXYZR32(cloud);
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
// Verify no LaserScan when PointCloud2 fields are missing 'x' and 'y'
publishR(cloud);
EXPECT_FALSE(waitForScan(ros::WallDuration(0.5)));
// Verify that the node hasn't crashed by sending normal PointCloud2 fields
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZIR1(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
ASSERT_EQ(cloud.header.stamp, g_scan.header.stamp);
}
// Verify that non-point fields are passed through unmodified
TEST(System, empty_data)
{
// Make sure system is connected
ASSERT_EQ(1, g_sub.getNumPublishers());
ASSERT_EQ(1, g_pub.getNumSubscribers());
// Create PointCloud with 16 rings
PointCloud cloud;
cloud.header.frame_id = "abcdefghijklmnopqrstuvwxyz";
cloud.points.resize(1);
cloud.points[0].x = 0.0;
cloud.points[0].y = 0.0;
cloud.points[0].z = 0.0;
cloud.points[0].i = 0.0;
cloud.points[0].r = 15;
// Verify that all three PointCloud2 types create proper default values
// PointXYZIR (expected format)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZIR1(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanEmpty(cloud, true);
// PointXYZIR (unexpected format with intensity)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZIR2(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanEmpty(cloud, true);
// PointXYZR (unexpected format without intensity)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZR(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanEmpty(cloud, false);
}
// Verify that every piece of a small amount of random data is passed through
TEST(System, random_data_sparse)
{
// Make sure system is connected
ASSERT_EQ(1, g_sub.getNumPublishers());
ASSERT_EQ(1, g_pub.getNumSubscribers());
// Create PointCloud with sparse random data
PointCloud cloud;
cloud.header.frame_id = "velodyne";
const size_t RANGE_COUNT = 100;
const size_t RING_COUNT = 16;
const double RANGE_MAX = 20.0;
const double INTENSITY_MAX = 1.0;
for (size_t i = 0; i < RANGE_COUNT; i++)
{
double angle_y = i * 1.99 * M_PI / RANGE_COUNT; // yaw
for (size_t j = 0; j < RING_COUNT; j++)
{
double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch
double range = std::rand() * (RANGE_MAX / RAND_MAX);
Point point;
point.x = range * cos(angle_p) * cos(angle_y);
point.y = range * cos(angle_p) * sin(angle_y);
point.z = range * sin(angle_p);
point.i = std::rand() * (INTENSITY_MAX / RAND_MAX);
point.r = j;
cloud.points.push_back(point);
}
}
// Verify that all three PointCloud2 types are handled correctly
// PointXYZIR (expected format)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZIR1(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanSparse(cloud, 8, RING_COUNT, true);
// PointXYZIR (unexpected format with intensity)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZIR2(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanSparse(cloud, 8, RING_COUNT, true);
// PointXYZR (unexpected format without intensity)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZR(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanSparse(cloud, 8, RING_COUNT, false);
}
// Verify that every LaserScan range is valid when given an extra large amount of random data
TEST(System, random_data_dense)
{
// Make sure system is connected
ASSERT_EQ(1, g_sub.getNumPublishers());
ASSERT_EQ(1, g_pub.getNumSubscribers());
// Create PointCloud with dense random data
PointCloud cloud;
cloud.header.frame_id = "velodyne";
const size_t RANGE_COUNT = 2500;
const size_t RING_COUNT = 16;
const double RANGE_MAX = 20.0;
const double INTENSITY_MAX = 1.0;
for (size_t i = 0; i < RANGE_COUNT; i++)
{
double angle_y = i * 2.0 * M_PI / RANGE_COUNT; // yaw
for (size_t j = 0; j < RING_COUNT; j++)
{
double angle_p = j * 0.2 * M_PI / RING_COUNT - 0.1 * M_PI; // pitch
double range = std::rand() * (RANGE_MAX / RAND_MAX);
Point point;
point.x = range * cos(angle_p) * cos(angle_y);
point.y = range * cos(angle_p) * sin(angle_y);
point.z = range * sin(angle_p);
point.i = std::rand() * (INTENSITY_MAX / RAND_MAX);
point.r = j;
cloud.points.push_back(point);
}
}
// Verify that all three PointCloud2 types are handled correctly
// PointXYZIR (expected format)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZIR1(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanDense(cloud, 8, true);
// PointXYZIR (unexpected format with intensity)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZIR2(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanDense(cloud, 8, true);
// PointXYZR (unexpected format without intensity)
cloud.header.stamp = rosTime(ros::WallTime::now());
publishXYZR(cloud);
ASSERT_TRUE(waitForScan(ros::WallDuration(1.0)));
verifyScanDense(cloud, 8, false);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
// Initialize ROS
ros::init(argc, argv, "test_lazy_subscriber");
ros::NodeHandle nh;
// Setup publisher and subscriber
g_pub = nh.advertise<sensor_msgs::PointCloud2>("velodyne_points", 2);
g_sub = nh.subscribe("scan", 2, recv);
// Wait for other nodes to startup
ros::WallDuration(1.0).sleep();
ros::spinOnce();
// Run all the tests that were declared with TEST()
return RUN_ALL_TESTS();
}
sensors/velodyne/velodyne_laserscan/tests/system_node.test
+17
View File
@@ -0,0 +1,17 @@
<!-- -*- mode: XML -*- -->
<!-- rostest of the velodyne_laserscan system -->
<launch>
<!-- Select log or screen output -->
<arg name="output" default="log"/> <!-- screen/log -->
<!-- Start the laserscan node -->
<node pkg="velodyne_laserscan" type="velodyne_laserscan_node" name="laserscan" output="$(arg output)" />
<!-- Start the rostest -->
<test test-name="test_system_node" pkg="velodyne_laserscan"
type="test_system_node" name="test_system">
</test>
</launch>
sensors/velodyne/velodyne_laserscan/tests/system_nodelet.test
+21
View File
@@ -0,0 +1,21 @@
<!-- -*- mode: XML -*- -->
<!-- rostest of the velodyne_laserscan system -->
<launch>
<!-- Select log or screen output -->
<arg name="output" default="log"/> <!-- screen/log -->
<!-- Start the laserscan nodelet -->
<arg name="manager" default="nodelet_manager" />
<node pkg="nodelet" type="nodelet" name="$(arg manager)" args="manager" output="$(arg output)" />
<node pkg="nodelet" type="nodelet" name="velodyne_laserscan" output="$(arg output)"
args="load velodyne_laserscan/LaserScanNodelet $(arg manager)">
</node>
<!-- Start the rostest -->
<test test-name="test_system_nodelet" pkg="velodyne_laserscan"
type="test_system_nodelet" name="test_system">
</test>
</launch>