AS: adding first phase of orientation
This commit is contained in:
Alexander Schaefer
@@ -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 @@
|
||||
0
|
||||
@@ -0,0 +1 @@
|
||||
# generated from colcon_core/shell/template/command_prefix.sh.em
|
||||
@@ -0,0 +1,20 @@
|
||||
AMENT_PREFIX_PATH=/opt/ros/humble
|
||||
COLCON=1
|
||||
HOME=/root
|
||||
HOSTNAME=a41c1d4d4b48
|
||||
LANG=C.UTF-8
|
||||
LC_ALL=C.UTF-8
|
||||
LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
|
||||
LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=30;41:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:
|
||||
OLDPWD=/root/ros2_ws/src
|
||||
PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin
|
||||
PWD=/root/ros2_ws/build/examples_rclpy_pointcloud_publisher
|
||||
PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
|
||||
ROS_DISTRO=humble
|
||||
ROS_LOCALHOST_ONLY=0
|
||||
ROS_PYTHON_VERSION=3
|
||||
ROS_VERSION=2
|
||||
SHLVL=1
|
||||
TERM=xterm
|
||||
_=/usr/bin/colcon
|
||||
_colcon_cd_root=/opt/ros/humble/
|
||||
@@ -0,0 +1 @@
|
||||
/root/ros2_ws/src/examples/rclpy/topics/pointcloud_publisher/examples_rclpy_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 @@
|
||||
|
||||
@@ -0,0 +1,14 @@
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher/__init__.py
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher/pointcloud_publisher.py
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher/__pycache__/__init__.cpython-310.pyc
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher/__pycache__/pointcloud_publisher.cpython-310.pyc
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/share/ament_index/resource_index/packages/examples_rclpy_pointcloud_publisher
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/share/examples_rclpy_pointcloud_publisher/package.xml
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher-0.15.3-py3.10.egg-info/zip-safe
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher-0.15.3-py3.10.egg-info/dependency_links.txt
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher-0.15.3-py3.10.egg-info/top_level.txt
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher-0.15.3-py3.10.egg-info/requires.txt
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher-0.15.3-py3.10.egg-info/entry_points.txt
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher-0.15.3-py3.10.egg-info/PKG-INFO
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/python3.10/site-packages/examples_rclpy_pointcloud_publisher-0.15.3-py3.10.egg-info/SOURCES.txt
|
||||
/root/ros2_ws/install/examples_rclpy_pointcloud_publisher/lib/examples_rclpy_pointcloud_publisher/pointcloud_publisher
|
||||
1
ros2_ws/build/examples_rclpy_pointcloud_publisher/package.xml
Symbolic link
1
ros2_ws/build/examples_rclpy_pointcloud_publisher/package.xml
Symbolic link
@@ -0,0 +1 @@
|
||||
/root/ros2_ws/src/examples/rclpy/topics/pointcloud_publisher/package.xml
|
||||
Binary file not shown.
@@ -0,0 +1,4 @@
|
||||
import sys
|
||||
if sys.prefix == '/usr':
|
||||
sys.real_prefix = sys.prefix
|
||||
sys.prefix = sys.exec_prefix = '/root/ros2_ws/install/examples_rclpy_pointcloud_publisher'
|
||||
@@ -0,0 +1 @@
|
||||
/root/ros2_ws/src/examples/rclpy/topics/pointcloud_publisher/resource/examples_rclpy_pointcloud_publisher
|
||||
1
ros2_ws/build/examples_rclpy_pointcloud_publisher/setup.cfg
Symbolic link
1
ros2_ws/build/examples_rclpy_pointcloud_publisher/setup.cfg
Symbolic link
@@ -0,0 +1 @@
|
||||
/root/ros2_ws/src/examples/rclpy/topics/pointcloud_publisher/setup.cfg
|
||||
@@ -0,0 +1 @@
|
||||
prepend-non-duplicate;PYTHONPATH;/root/ros2_ws/build/examples_rclpy_pointcloud_publisher
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
|
||||
|
||||
colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\/root/ros2_ws/build/examples_rclpy_pointcloud_publisher"
|
||||
@@ -0,0 +1,3 @@
|
||||
# generated from colcon_core/shell/template/hook_prepend_value.sh.em
|
||||
|
||||
_colcon_prepend_unique_value PYTHONPATH "/root/ros2_ws/build/examples_rclpy_pointcloud_publisher"
|
||||
Reference in New Issue
Block a user