diff --git a/src/test/point_cloud_test.py b/src/test/point_cloud_test.py new file mode 100644 index 0000000000..a410c9c33b --- /dev/null +++ b/src/test/point_cloud_test.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python + +# This program publishes a pointcloud2 to test rviz rendering + +from __future__ import print_function + +import rospy +import numpy as np +from sensor_msgs import point_cloud2 +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from std_msgs.msg import Header + +import math +import time +from geometry_msgs.msg import TransformStamped +from tf2_ros.transform_broadcaster import TransformBroadcaster +from tf.transformations import quaternion_from_euler +from visualization_msgs.msg import Marker + +RATE = 30 +width = 100 +height = 100 + + +def generate_point_cloud(): + fields = [ + PointField("x", 0, PointField.FLOAT32, 1), + PointField("y", 4, PointField.FLOAT32, 1), + PointField("z", 8, PointField.FLOAT32, 1), + PointField("intensity", 12, PointField.FLOAT32, 1), + ] + + header = Header() + header.frame_id = "map" + header.stamp = rospy.Time.now() + + x, y = np.meshgrid(np.linspace(-2, 2, width), np.linspace(-2, 2, height)) + z = 0.5 * np.sin(2 * x) * np.sin(3 * y) + points = np.array([x, y, z, z]).reshape(4, -1).T + + return point_cloud2.create_cloud(header, fields, points) + + +def draw_eight(elapsed_time, full_cycle_duration=10, scale=10): + # lemniscate of Gerono + progress = (elapsed_time % full_cycle_duration) / full_cycle_duration + t = -0.5 * math.pi + progress * (2 * math.pi) + x = math.cos(t) * scale + y = math.sin(t) * math.cos(t) * scale + dx_dt = -math.sin(t) + dy_dt = math.cos(t) * math.cos(t) - math.sin(t) * math.sin(t) + yaw = math.atan2(dy_dt, dx_dt) + return x, y, yaw + + +def display_dummy_robot(stamp): + marker = Marker() + marker.header.frame_id = "base_link" + marker.header.stamp = stamp + marker.ns = "robot" + marker.id = 0 + marker.type = Marker.CUBE + marker.action = Marker.ADD + marker.pose.position.x = 0 + marker.pose.position.y = 0 + marker.pose.position.z = 0 + marker.pose.orientation.x = 0.0 + marker.pose.orientation.y = 0.0 + marker.pose.orientation.z = 0.0 + marker.pose.orientation.w = 1.0 + marker.scale.x = 1 + marker.scale.y = 1 + marker.scale.z = 1 + marker.color.a = 1.0 + marker.color.r = 0.0 + marker.color.g = 1.0 + marker.color.b = 0.0 + vis_pub.publish(marker) + + +if __name__ == "__main__": + rospy.init_node("point_cloud_test") + + tf_broadcaster = TransformBroadcaster() + + vis_pub = rospy.Publisher("robot/marker", Marker, queue_size=5) + pc_pub = rospy.Publisher("points2", PointCloud2, queue_size=5) + pc = generate_point_cloud() + + i = 0 + rate = rospy.Rate(RATE) + start = rospy.Time.now() + while not rospy.is_shutdown(): + now = rospy.Time.now() + + if i % RATE == 0: + # publish just once per second + pc.header.stamp = now + pc_pub.publish(pc) + i += 1 + + robot_x, robot_y, robot_yaw = draw_eight((now - start).to_sec()) + q = quaternion_from_euler(0, 0, robot_yaw) + + tf = TransformStamped() + tf.header.frame_id = "map" + tf.header.stamp = now + tf.child_frame_id = "base_link" + tf.transform.translation.x = robot_x + tf.transform.translation.y = robot_y + tf.transform.rotation.x = q[0] + tf.transform.rotation.y = q[1] + tf.transform.rotation.z = q[2] + tf.transform.rotation.w = q[3] + tf_broadcaster.sendTransform(tf) + + display_dummy_robot(now) + + rate.sleep() diff --git a/src/test/point_cloud_test.rviz b/src/test/point_cloud_test.rviz new file mode 100644 index 0000000000..f6d68c3465 --- /dev/null +++ b/src/test/point_cloud_test.rviz @@ -0,0 +1,155 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 818 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: map + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: /points2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /robot/marker + Name: Marker + Namespaces: + robot: true + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 30.52842903137207 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6153981685638428 + Target Frame: + Yaw: 1.025397777557373 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1115 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000003bdfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003bd000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003bd000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f000003bd00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 0