-
Notifications
You must be signed in to change notification settings - Fork 191
/
Copy pathsample_pointcloud_screenpoint.launch
46 lines (41 loc) · 1.69 KB
/
sample_pointcloud_screenpoint.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
<launch>
<arg name="gui" default="true"/>
<arg name="publish_rate" default="1"/>
<include file="$(find jsk_pcl_ros_utils)/sample/include/play_rosbag_shelf_bin.xml"/>
<arg name="MANAGER" value="/right_hand_camera/right_hand_camera_nodelet_manager" />
<node name="sample_point_publisher"
pkg="rostopic" type="rostopic"
args="pub -r $(arg publish_rate) -s /sample_point_publisher/output/screenpoint geometry_msgs/PointStamped
'{header: {stamp: now, frame_id: right_hand_camera_rgb_optical_frame},
point: {x: 100, y: 100, z: 0}}'"/>
<node name="pointcloud_screenpoint"
pkg="nodelet" type="nodelet"
args="load jsk_pcl/PointcloudScreenpoint $(arg MANAGER)"
respawn="true">
<remap from="~points" to="/right_hand_camera/depth_registered/points"/>
<remap from="~point" to="sample_point_publisher/output/screenpoint"/>
<rosparam>
queue_size: 100
use_point: true
publish_points: true
publish_point: true
</rosparam>
</node>
<node name="pointcloud_screenpoint_client"
pkg="jsk_pcl_ros" type="pointcloud_screenpoint.l"
respawn="true">
<remap from="ray_marker_array" to="~output/ray_marker_array"/>
<remap from="image_marker" to="~output/image_marker"/>
<remap from="ray_coords" to="~output/ray_coords"/>
<rosparam>
sensor_topic: sample_point_publisher/output
ray_srv: pointcloud_screenpoint/screen_to_point
base_frame: right_hand_camera_rgb_optical_frame
</rosparam>
</node>
<group if="$(arg gui)">
<node name="rviz"
pkg="rviz" type="rviz"
args="-d $(find jsk_pcl_ros)/sample/pointcloud_screenpoint.rviz"/>
</group>
</launch>