AS: adding first phase of orientation
This commit is contained in:
Alexander Schaefer
@@ -0,0 +1,33 @@
|
||||
#!/usr/bin/python3
|
||||
# EASY-INSTALL-ENTRY-SCRIPT: 'examples-rclpy-pointcloud-publisher==0.15.3','console_scripts','pointcloud_publisher'
|
||||
import re
|
||||
import sys
|
||||
|
||||
# for compatibility with easy_install; see #2198
|
||||
__requires__ = 'examples-rclpy-pointcloud-publisher==0.15.3'
|
||||
|
||||
try:
|
||||
from importlib.metadata import distribution
|
||||
except ImportError:
|
||||
try:
|
||||
from importlib_metadata import distribution
|
||||
except ImportError:
|
||||
from pkg_resources import load_entry_point
|
||||
|
||||
|
||||
def importlib_load_entry_point(spec, group, name):
|
||||
dist_name, _, _ = spec.partition('==')
|
||||
matches = (
|
||||
entry_point
|
||||
for entry_point in distribution(dist_name).entry_points
|
||||
if entry_point.group == group and entry_point.name == name
|
||||
)
|
||||
return next(matches).load()
|
||||
|
||||
|
||||
globals().setdefault('load_entry_point', importlib_load_entry_point)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
|
||||
sys.exit(load_entry_point('examples-rclpy-pointcloud-publisher==0.15.3', 'console_scripts', 'pointcloud_publisher')())
|
||||
@@ -0,0 +1,12 @@
|
||||
Metadata-Version: 2.1
|
||||
Name: examples-rclpy-pointcloud-publisher
|
||||
Version: 0.15.3
|
||||
Summary: Example on how to publish a Pointcloud2 message
|
||||
Home-page: UNKNOWN
|
||||
Maintainer: Aditya Pande, Shane Loretz
|
||||
Maintainer-email: aditya.pande@openrobotics.org, shane@openrobotics.org
|
||||
License: Apache 2.0
|
||||
Platform: UNKNOWN
|
||||
|
||||
UNKNOWN
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
package.xml
|
||||
setup.cfg
|
||||
setup.py
|
||||
../../../../../build/examples_rclpy_pointcloud_publisher/examples_rclpy_pointcloud_publisher.egg-info/PKG-INFO
|
||||
../../../../../build/examples_rclpy_pointcloud_publisher/examples_rclpy_pointcloud_publisher.egg-info/SOURCES.txt
|
||||
../../../../../build/examples_rclpy_pointcloud_publisher/examples_rclpy_pointcloud_publisher.egg-info/dependency_links.txt
|
||||
../../../../../build/examples_rclpy_pointcloud_publisher/examples_rclpy_pointcloud_publisher.egg-info/entry_points.txt
|
||||
../../../../../build/examples_rclpy_pointcloud_publisher/examples_rclpy_pointcloud_publisher.egg-info/requires.txt
|
||||
../../../../../build/examples_rclpy_pointcloud_publisher/examples_rclpy_pointcloud_publisher.egg-info/top_level.txt
|
||||
../../../../../build/examples_rclpy_pointcloud_publisher/examples_rclpy_pointcloud_publisher.egg-info/zip-safe
|
||||
examples_rclpy_pointcloud_publisher/__init__.py
|
||||
examples_rclpy_pointcloud_publisher/pointcloud_publisher.py
|
||||
resource/examples_rclpy_pointcloud_publisher
|
||||
test/test_copyright.py
|
||||
test/test_flake8.py
|
||||
test/test_pep257.py
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
[console_scripts]
|
||||
pointcloud_publisher = examples_rclpy_pointcloud_publisher.pointcloud_publisher:main
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
setuptools
|
||||
@@ -0,0 +1 @@
|
||||
examples_rclpy_pointcloud_publisher
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,70 @@
|
||||
# Copyright 2020 Evan Flynn
|
||||
#
|
||||
# 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.
|
||||
|
||||
import numpy as np
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
from sensor_msgs.msg import PointField
|
||||
from sensor_msgs_py import point_cloud2
|
||||
from std_msgs.msg import Header
|
||||
|
||||
|
||||
class PointCloudPublisher(Node):
|
||||
|
||||
rate = 30
|
||||
moving = True
|
||||
width = 100
|
||||
height = 100
|
||||
|
||||
header = Header()
|
||||
header.frame_id = 'map'
|
||||
|
||||
dtype = PointField.FLOAT32
|
||||
point_step = 16
|
||||
fields = [PointField(name='x', offset=0, datatype=dtype, count=1),
|
||||
PointField(name='y', offset=4, datatype=dtype, count=1),
|
||||
PointField(name='z', offset=8, datatype=dtype, count=1),
|
||||
PointField(name='intensity', offset=12, datatype=dtype, count=1)]
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('pc_publisher')
|
||||
self.publisher_ = self.create_publisher(PointCloud2, 'test_cloud', 10)
|
||||
timer_period = 1 / self.rate
|
||||
self.timer = self.create_timer(timer_period, self.timer_callback)
|
||||
self.counter = 0
|
||||
|
||||
def timer_callback(self):
|
||||
self.header.stamp = self.get_clock().now().to_msg()
|
||||
x, y = np.meshgrid(np.linspace(-2, 2, self.width), np.linspace(-2, 2, self.height))
|
||||
z = 0.5 * np.sin(2*x-self.counter/10.0) * np.sin(2*y)
|
||||
points = np.array([x, y, z, z]).reshape(4, -1).T
|
||||
pc2_msg = point_cloud2.create_cloud(self.header, self.fields, points)
|
||||
self.publisher_.publish(pc2_msg)
|
||||
|
||||
if self.moving:
|
||||
self.counter += 1
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
pc_publisher = PointCloudPublisher()
|
||||
rclpy.spin(pc_publisher)
|
||||
pc_publisher.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1 @@
|
||||
python3-numpy:rclpy:sensor_msgs:sensor_msgs_py:std_msgs
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;AMENT_PREFIX_PATH;
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;PYTHONPATH;lib/python3.10/site-packages
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\lib/python3.10/site-packages"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.10/site-packages"
|
||||
@@ -0,0 +1,31 @@
|
||||
# generated from colcon_bash/shell/template/package.bash.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# a bash script is able to determine its own path if necessary
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
_colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
|
||||
else
|
||||
_colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_bash_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# source sh script of this package
|
||||
_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/examples_rclpy_pointcloud_publisher/package.sh"
|
||||
|
||||
unset _colcon_package_bash_source_script
|
||||
unset _colcon_package_bash_COLCON_CURRENT_PREFIX
|
||||
@@ -0,0 +1,6 @@
|
||||
source;share/examples_rclpy_pointcloud_publisher/hook/pythonpath.ps1
|
||||
source;share/examples_rclpy_pointcloud_publisher/hook/pythonpath.dsv
|
||||
source;share/examples_rclpy_pointcloud_publisher/hook/pythonpath.sh
|
||||
source;share/examples_rclpy_pointcloud_publisher/hook/ament_prefix_path.ps1
|
||||
source;share/examples_rclpy_pointcloud_publisher/hook/ament_prefix_path.dsv
|
||||
source;share/examples_rclpy_pointcloud_publisher/hook/ament_prefix_path.sh
|
||||
@@ -0,0 +1,116 @@
|
||||
# generated from colcon_powershell/shell/template/package.ps1.em
|
||||
|
||||
# function to append a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as leading separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
function colcon_append_unique_value {
|
||||
param (
|
||||
$_listname,
|
||||
$_value
|
||||
)
|
||||
|
||||
# get values from variable
|
||||
if (Test-Path Env:$_listname) {
|
||||
$_values=(Get-Item env:$_listname).Value
|
||||
} else {
|
||||
$_values=""
|
||||
}
|
||||
$_duplicate=""
|
||||
# start with no values
|
||||
$_all_values=""
|
||||
# iterate over existing values in the variable
|
||||
if ($_values) {
|
||||
$_values.Split(";") | ForEach {
|
||||
# not an empty string
|
||||
if ($_) {
|
||||
# not a duplicate of _value
|
||||
if ($_ -eq $_value) {
|
||||
$_duplicate="1"
|
||||
}
|
||||
if ($_all_values) {
|
||||
$_all_values="${_all_values};$_"
|
||||
} else {
|
||||
$_all_values="$_"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
# append only non-duplicates
|
||||
if (!$_duplicate) {
|
||||
# avoid leading separator
|
||||
if ($_all_values) {
|
||||
$_all_values="${_all_values};${_value}"
|
||||
} else {
|
||||
$_all_values="${_value}"
|
||||
}
|
||||
}
|
||||
|
||||
# export the updated variable
|
||||
Set-Item env:\$_listname -Value "$_all_values"
|
||||
}
|
||||
|
||||
# function to prepend a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as trailing separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
function colcon_prepend_unique_value {
|
||||
param (
|
||||
$_listname,
|
||||
$_value
|
||||
)
|
||||
|
||||
# get values from variable
|
||||
if (Test-Path Env:$_listname) {
|
||||
$_values=(Get-Item env:$_listname).Value
|
||||
} else {
|
||||
$_values=""
|
||||
}
|
||||
# start with the new value
|
||||
$_all_values="$_value"
|
||||
# iterate over existing values in the variable
|
||||
if ($_values) {
|
||||
$_values.Split(";") | ForEach {
|
||||
# not an empty string
|
||||
if ($_) {
|
||||
# not a duplicate of _value
|
||||
if ($_ -ne $_value) {
|
||||
# keep non-duplicate values
|
||||
$_all_values="${_all_values};$_"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
# export the updated variable
|
||||
Set-Item env:\$_listname -Value "$_all_values"
|
||||
}
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
function colcon_package_source_powershell_script {
|
||||
param (
|
||||
$_colcon_package_source_powershell_script
|
||||
)
|
||||
# source script with conditional trace output
|
||||
if (Test-Path $_colcon_package_source_powershell_script) {
|
||||
if ($env:COLCON_TRACE) {
|
||||
echo ". '$_colcon_package_source_powershell_script'"
|
||||
}
|
||||
. "$_colcon_package_source_powershell_script"
|
||||
} else {
|
||||
Write-Error "not found: '$_colcon_package_source_powershell_script'"
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
# a powershell script is able to determine its own path
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
|
||||
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/examples_rclpy_pointcloud_publisher/hook/pythonpath.ps1"
|
||||
colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/examples_rclpy_pointcloud_publisher/hook/ament_prefix_path.ps1"
|
||||
|
||||
Remove-Item Env:\COLCON_CURRENT_PREFIX
|
||||
@@ -0,0 +1,87 @@
|
||||
# generated from colcon_core/shell/template/package.sh.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# function to prepend a value to a variable
|
||||
# which uses colons as separators
|
||||
# duplicates as well as trailing separators are avoided
|
||||
# first argument: the name of the result variable
|
||||
# second argument: the value to be prepended
|
||||
_colcon_prepend_unique_value() {
|
||||
# arguments
|
||||
_listname="$1"
|
||||
_value="$2"
|
||||
|
||||
# get values from variable
|
||||
eval _values=\"\$$_listname\"
|
||||
# backup the field separator
|
||||
_colcon_prepend_unique_value_IFS=$IFS
|
||||
IFS=":"
|
||||
# start with the new value
|
||||
_all_values="$_value"
|
||||
# workaround SH_WORD_SPLIT not being set in zsh
|
||||
if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
|
||||
colcon_zsh_convert_to_array _values
|
||||
fi
|
||||
# iterate over existing values in the variable
|
||||
for _item in $_values; do
|
||||
# ignore empty strings
|
||||
if [ -z "$_item" ]; then
|
||||
continue
|
||||
fi
|
||||
# ignore duplicates of _value
|
||||
if [ "$_item" = "$_value" ]; then
|
||||
continue
|
||||
fi
|
||||
# keep non-duplicate values
|
||||
_all_values="$_all_values:$_item"
|
||||
done
|
||||
unset _item
|
||||
# restore the field separator
|
||||
IFS=$_colcon_prepend_unique_value_IFS
|
||||
unset _colcon_prepend_unique_value_IFS
|
||||
# export the updated variable
|
||||
eval export $_listname=\"$_all_values\"
|
||||
unset _all_values
|
||||
unset _values
|
||||
|
||||
unset _value
|
||||
unset _listname
|
||||
}
|
||||
|
||||
# since a plain shell script can't determine its own path when being sourced
|
||||
# either use the provided COLCON_CURRENT_PREFIX
|
||||
# or fall back to the build time prefix (if it exists)
|
||||
_colcon_package_sh_COLCON_CURRENT_PREFIX="/root/ros2_ws/install/examples_rclpy_pointcloud_publisher"
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
|
||||
echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
|
||||
unset _colcon_package_sh_COLCON_CURRENT_PREFIX
|
||||
return 1
|
||||
fi
|
||||
COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
unset _colcon_package_sh_COLCON_CURRENT_PREFIX
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_sh_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# source sh hooks
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/examples_rclpy_pointcloud_publisher/hook/pythonpath.sh"
|
||||
_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/examples_rclpy_pointcloud_publisher/hook/ament_prefix_path.sh"
|
||||
|
||||
unset _colcon_package_sh_source_script
|
||||
unset COLCON_CURRENT_PREFIX
|
||||
|
||||
# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
|
||||
@@ -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>examples_rclpy_pointcloud_publisher</name>
|
||||
<version>0.15.3</version>
|
||||
<description>Example on how to publish a Pointcloud2 message</description>
|
||||
<maintainer email="evanflynn.msu@gmail.com">Evan Flynn</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs_py</exec_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_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>
|
||||
@@ -0,0 +1,42 @@
|
||||
# generated from colcon_zsh/shell/template/package.zsh.em
|
||||
|
||||
# This script extends the environment for this package.
|
||||
|
||||
# a zsh script is able to determine its own path if necessary
|
||||
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
|
||||
# the prefix is two levels up from the package specific share directory
|
||||
_colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
|
||||
else
|
||||
_colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
|
||||
fi
|
||||
|
||||
# function to source another script with conditional trace output
|
||||
# first argument: the path of the script
|
||||
# additional arguments: arguments to the script
|
||||
_colcon_package_zsh_source_script() {
|
||||
if [ -f "$1" ]; then
|
||||
if [ -n "$COLCON_TRACE" ]; then
|
||||
echo "# . \"$1\""
|
||||
fi
|
||||
. "$@"
|
||||
else
|
||||
echo "not found: \"$1\"" 1>&2
|
||||
fi
|
||||
}
|
||||
|
||||
# function to convert array-like strings into arrays
|
||||
# to workaround SH_WORD_SPLIT not being set
|
||||
colcon_zsh_convert_to_array() {
|
||||
local _listname=$1
|
||||
local _dollar="$"
|
||||
local _split="{="
|
||||
local _to_array="(\"$_dollar$_split$_listname}\")"
|
||||
eval $_listname=$_to_array
|
||||
}
|
||||
|
||||
# source sh script of this package
|
||||
_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/examples_rclpy_pointcloud_publisher/package.sh"
|
||||
unset convert_zsh_to_array
|
||||
|
||||
unset _colcon_package_zsh_source_script
|
||||
unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
|
||||
Reference in New Issue
Block a user