From 4da6a872f9d4737f15d628f802e787162355f455 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sun, 3 Dec 2023 16:48:18 +0900 Subject: [PATCH] [elevator_move_base_pr2] add debug programs. launch debug_detector.launch at first, then run sample/debug.l on roseus interpreter --- .../launch/debug_detector.launch | 219 ++++++++++++++++++ .../node_scripts/publish_dummy_camera_info.py | 41 ++++ elevator_move_base_pr2/sample/debug.l | 8 + 3 files changed, 268 insertions(+) create mode 100644 elevator_move_base_pr2/launch/debug_detector.launch create mode 100755 elevator_move_base_pr2/node_scripts/publish_dummy_camera_info.py create mode 100644 elevator_move_base_pr2/sample/debug.l diff --git a/elevator_move_base_pr2/launch/debug_detector.launch b/elevator_move_base_pr2/launch/debug_detector.launch new file mode 100644 index 0000000000..144ea26dec --- /dev/null +++ b/elevator_move_base_pr2/launch/debug_detector.launch @@ -0,0 +1,219 @@ + + + + + + + + + + + + + + + + + + + + + update_rate: 2.0 + + + + + + + + + + + + + + default_duration: 0.0 # infinite + + + + + + + + + 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 + + + + + + + + + + + + + + + + + + + + + + + red: 253 + green: 251 + blue: 240 + decay_r: 0.1 + decay_g: 0.1 + decay_b: 0.05 + + + + + + + + + default_duration: 0.0 # infinite + + + + + + + + label_names: + - _background_ + - button + bg_label: 0 + approximate_sync: false + queue_size: 100 + + + + + + + + + + + + + + + + + + + + + label_value: 1 + + + + + + + + + + + + use_asynchronous: true + organize_cloud: true + + + + + + + + approximate_sync: true + queue_size: 1000 + keep_organized: true + + + + + + + min_size: 100 + max_size: 10000 + tolerance: 0.02 + + + + + + + + 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) + + + + + diff --git a/elevator_move_base_pr2/node_scripts/publish_dummy_camera_info.py b/elevator_move_base_pr2/node_scripts/publish_dummy_camera_info.py new file mode 100755 index 0000000000..1f10a555e0 --- /dev/null +++ b/elevator_move_base_pr2/node_scripts/publish_dummy_camera_info.py @@ -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() diff --git a/elevator_move_base_pr2/sample/debug.l b/elevator_move_base_pr2/sample/debug.l new file mode 100644 index 0000000000..97af072482 --- /dev/null +++ b/elevator_move_base_pr2/sample/debug.l @@ -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")))