Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[DO NOT MERGE] Subway2023 debug #3

Open
wants to merge 1 commit into
base: subway2023
Choose a base branch
from
Open

[DO NOT MERGE] Subway2023 debug #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
219 changes: 219 additions & 0 deletions elevator_move_base_pr2/launch/debug_detector.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,219 @@
<?xml version="1.0" encoding="utf-8"?>
<launch>
<arg name="MANAGER" default="rosbag_play_nodelet_manager" />
<arg name="scene" default="eng2" />
<arg name="launch_find_elevator_button" default="true" />

<node name="elevator_move_base" pkg="roseus" type="roseus"
output="screen" respawn="true"
args="$(find elevator_move_base_pr2)/src/elevator-move-base.l"/>

<!-- Publish dummy camera_info because the rosbag not includes the camerainfo -->
<node name="publish_dummy_camera_info" pkg="elevator_move_base_pr2" type="publish_dummy_camera_info.py"/>

<!-- decompress narrow stereos -->
<node name="narrow_stereo_left_decompress" pkg="image_transport" type="republish"
args="compressed in:=/narrow_stereo/left/image_rect raw out:=/narrow_stereo/left/image_rect" />
<node name="narrow_stereo_right_decompress" pkg="image_transport" type="republish"
args="compressed in:=/narrow_stereo/right/image_rect raw out:=/narrow_stereo/right/image_rect" />

<group ns="/narrow_stereo/left">
<node name="lightweight_throttle"
pkg="nodelet" type="nodelet"
args="load jsk_topic_tools/LightweightThrottle /$(arg MANAGER)"
>
<remap from="~input" to="image_rect"/>
<remap from="~output" to="image_rect_throttle"/>
<rosparam>
update_rate: 2.0
</rosparam>
</node>
<node name="imagesift"
pkg="nodelet" type="nodelet"
args="load imagesift/ImageSift /$(arg MANAGER)"
launch-prefix="nice -n +10">
<remap from="image" to="image_rect_throttle"/>
</node>
<!-- SUBSTITUTE the include file below -->
<!-- <include file="$(find elevator_move_base_pr2)/launch/elevator_panels_detection_$(arg scene).launch"> -->
<!-- <arg name="INPUT_FEATURE" value="ImageFeature0D"/> -->
<!-- <arg name="MANAGER" value="/$(arg MANAGER)"/> -->
<!-- </include> -->
<node name="pass_through_feature_to_call_panel"
pkg="nodelet" type="nodelet"
args="load jsk_topic_tools/Passthrough $(arg MANAGER)">
<remap from="~input" to="ImageFeature0D"/>
<rosparam>
default_duration: 0.0 # infinite
</rosparam>
</node>
<node name="point_pose_extractor_elevator_call_panel"
pkg="jsk_perception" type="point_pose_extractor">
<!-- input -->
<remap from="ImageFeature0D" to="pass_through_feature_to_call_panel/output"/>
<!-- output -->
<remap from="~debug_image" to="point_pose_extractor_elevator_panels/debug_image"/>
<rosparam subst_value="true">
template_filename: $(find jsk_perception)/template/elevator_call_panel_eng2.png
child_frame_id: matching
object_width: 0.098
object_height: 0.241
reprojection_threshold: 1.0
distanceratio_threshold: 0.8
error_threshold: 60.0
theta_step: 0.2
phi_step: 5.0
relative_pose: "0.1205 0.049 -0.0005 0.0 -0.707107 0.0 0.707107"
viewer_window: false
window_name: elevator_call_panel
</rosparam>
</node>
<node if="$(arg launch_find_elevator_button)" name="find_elevator_button"
pkg="elevator_move_base_pr2" type="find-elevator-button.l"
respawn="true" output="screen"/>
<!-- END SUBSTITUTE -->
</group>

<!-- Check if button is lit or not. -->
<group ns="/wide_stereo/left">
<!-- <node name="pass_through_image_rect_color" -->
<!-- pkg="nodelet" type="nodelet" -->
<!-- args="load jsk_topic_tools/Passthrough /$(arg MANAGER)"> -->
<!-- <remap from="~input" to="image_rect_color"/> -->
<!-- <remap from="~output" to="pass_through_image_rect_color_output"/> -->
<!-- <rosparam> -->
<!-- default_duration: 0.0 # infinite -->
<!-- </rosparam> -->
<!-- </node> -->
</group>
<node name="light_detector"
pkg="elevator_move_base_pr2" type="color_point_detector">
<!-- <remap from="image" to="/wide_stereo/left/pass_through_image_rect_color_output"/> -->
<remap from="image" to="/wide_stereo/left/image_rect_color"/>
<rosparam>
red: 253
green: 251
blue: 240
decay_r: 0.1
decay_g: 0.1
decay_b: 0.05
</rosparam>
</node>

<group ns="door_button">
<!-- Subscribe only when it gets service request -->
<node name="pass_through_image"
pkg="nodelet" type="nodelet"
args="standalone jsk_topic_tools/Passthrough"
output="screen" respawn="true">
<remap from="~input" to="/narrow_stereo/left/image_rect_color"/>
<rosparam>
default_duration: 0.0 # infinite
</rosparam>
</node>
<!-- Visualization -->
<node name="label_image_decomposer"
pkg="jsk_perception" type="label_image_decomposer.py"
output="screen" respawn="true">
<remap from="~input" to="/narrow_stereo/left/image_rect_color"/>
<remap from="~input/label" to="fcn_object_segmentation/output"/>
<rosparam>
label_names:
- _background_
- button
bg_label: 0
approximate_sync: false
queue_size: 100
</rosparam>
</node>
</group> <!-- ns: door_button -->


<!-- <include file="$(find elevator_move_base_pr2)/launch/door_button_segmentation_3d.launch"> -->
<!-- </include> -->
<arg name="DOOR_MANAGER" default="door_button_segmentation_manager"/>
<arg name="INPUT_LABEL" default="fcn_object_segmentation/output"/>
<arg name="INPUT_INFO" default="/narrow_stereo/left/camera_info"/>
<arg name="INPUT_CLOUD" default="/kinect_head/depth_registered/points"/>
<arg name="FIXED_FRAME" default="/head_mount_kinect_rgb_optical_frame"/>
<group ns="door_button">
<node name="$(arg DOOR_MANAGER)"
pkg="nodelet" type="nodelet"
args="manager"
output="screen" respawn="true">
</node>
<!-- Pipeline to get 3-dimentional bounding box of door button -->
<!-- label -> mask -> indices -> cloud -> cluster indices -> bboxes -->
<node name="label_to_mask"
pkg="nodelet" type="nodelet"
args="load jsk_perception/LabelToMaskImage $(arg DOOR_MANAGER)"
output="screen" respawn="true">
<remap from="~input" to="$(arg INPUT_LABEL)"/>
<rosparam>
label_value: 1
</rosparam>
</node>

<node name="mask_to_point_indices"
pkg="nodelet" type="nodelet"
args="load jsk_pcl_utils/MaskImageToPointIndices $(arg DOOR_MANAGER)"
output="screen" respawn="true">
<remap from="~input" to="label_to_mask/output"/>
</node>

<node name="depth_image_creator"
pkg="nodelet" type="nodelet"
args="load jsk_pcl/DepthImageCreator $(arg DOOR_MANAGER)"
output="screen" respawn="true">
<remap from="~input" to="$(arg INPUT_CLOUD)"/>
<remap from="~info" to="$(arg INPUT_INFO)"/>
<rosparam>
use_asynchronous: true
organize_cloud: true
</rosparam>
</node>

<node name="extract_indices"
pkg="nodelet" type="nodelet"
args="load jsk_pcl/ExtractIndices $(arg DOOR_MANAGER)"
output="screen" respawn="true">
<remap from="~input" to="depth_image_creator/output_cloud"/>
<remap from="~indices" to="mask_to_point_indices/output"/>
<rosparam>
approximate_sync: true
queue_size: 1000
keep_organized: true
</rosparam>
</node>

<node name="euclidean_clustering"
pkg="nodelet" type="nodelet"
args="load jsk_pcl/EuclideanClustering $(arg DOOR_MANAGER)"
output="screen" respawn="true">
<remap from="~input" to="extract_indices/output"/>
<rosparam>
min_size: 100
max_size: 10000
tolerance: 0.02
</rosparam>
</node>

<node name="cluster_indices_decomposer"
pkg="nodelet" type="nodelet"
args="load jsk_pcl/ClusterPointIndicesDecomposer $(arg DOOR_MANAGER)"
output="screen" respawn="true">
<remap from="~input" to="extract_indices/output"/>
<remap from="~target" to="euclidean_clustering/output"/>
<rosparam subst_value="true">
approximate_sync: false
queue_size: 1000
sort_by: -cloud_size
align_boxes: true
align_boxes_with_plane: false
use_pca: true
target_frame_id: $(arg FIXED_FRAME)
</rosparam>
</node>
</group> <!-- ns: $(arg NS) -->

</launch>
41 changes: 41 additions & 0 deletions elevator_move_base_pr2/node_scripts/publish_dummy_camera_info.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
#!/usr/bin/env python

import rospy
from sensor_msgs.msg import CameraInfo, Image
from sensor_msgs.msg import RegionOfInterest
# from std_msgs.msg import Header

def publish_dummy_camera_info(msg):
camera_info_msg = CameraInfo()
camera_info_msg.header = msg.header
camera_info_msg.height = 480
camera_info_msg.width = 640
camera_info_msg.distortion_model = "plumb_bob"
camera_info_msg.D = [-0.31774, 0.09207000000000001, 0.0004, -0.00051, 0.0]
camera_info_msg.K = [423.51537, 0.0, 319.81218, 0.0, 423.53835, 246.13077, 0.0, 0.0, 1.0]
camera_info_msg.R = [0.9999000000000001, -0.00489, 0.01293, 0.00489, 0.99999, 1e-05, -0.01293, 6.000000000000001e-05, 0.99992]
camera_info_msg.P = [386.17852, 0.0, 321.4065, 0.0, 0.0, 386.17852, 241.10974, 0.0, 0.0, 0.0, 1.0, 0.0]
camera_info_msg.binning_x = 0
camera_info_msg.binning_y = 0
roi = RegionOfInterest()
roi.x_offset = 0
roi.y_offset = 0
roi.height = 0
roi.width = 0
roi.do_rectify = False
camera_info_msg.roi = roi
pub.publish(camera_info_msg)


def main():
global pub
rospy.init_node("dummy_camera_info_publisher")
pub = rospy.Publisher("/wide_stereo/left/camera_info", CameraInfo, queue_size=1)
rospy.Subscriber(
"/wide_stereo/left/image_rect_color",
Image,
publish_dummy_camera_info
)
rospy.spin()

main()
8 changes: 8 additions & 0 deletions elevator_move_base_pr2/sample/debug.l
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,8 @@
#!/usr/bin/env roseus

(load "package://elevator_move_base_pr2/src/state/push-elevator-button.l")
(load "package://elevator_move_base_pr2/src/state-machine-main.l")

(initialize-env)

(look-button-state '((panel-name . "/elevator_call_panel")))
Loading