diff --git a/elevator_move_base_pr2.rosinstall b/elevator_move_base_pr2.rosinstall new file mode 100644 index 0000000000..836cc7a627 --- /dev/null +++ b/elevator_move_base_pr2.rosinstall @@ -0,0 +1,4 @@ +- git: + local-name: knorth55/coral_usb_ros + uri: https://github.com/knorth55/coral_usb_ros.git + version: master diff --git a/elevator_move_base_pr2/CMakeLists.txt b/elevator_move_base_pr2/CMakeLists.txt index 39dd773cbc..3ccdb8b079 100644 --- a/elevator_move_base_pr2/CMakeLists.txt +++ b/elevator_move_base_pr2/CMakeLists.txt @@ -1,54 +1,50 @@ cmake_minimum_required(VERSION 2.8.3) project(elevator_move_base_pr2) -find_package(catkin REQUIRED roscpp image_transport cv_bridge image_geometry tf geometry_msgs std_msgs message_generation roseus) - -add_message_files( - FILES Float32Stamped.msg) +find_package(catkin REQUIRED + cv_bridge + geometry_msgs + image_geometry + image_transport + roscpp + std_msgs + tf +) -generate_messages( - DEPENDENCIES std_msgs) +add_custom_target( + ${PROJECT_NAME}_install_trained_model + ALL + COMMAND ${PROJECT_SOURCE_DIR}/scripts/install_trained_model.py +) include_directories(${catkin_INCLUDE_DIRS}) catkin_package( - DEPENDS - CATKIN_DEPENDS - INCLUDE_DIRS - LIBRARIES + DEPENDS + CATKIN_DEPENDS + INCLUDE_DIRS + LIBRARIES ) add_executable(color_point_detector src/color_point_detector.cpp) target_link_libraries(color_point_detector ${catkin_LIBRARIES}) -catkin_package( - DEPENDS # - CATKIN_DEPENDS # TODO - INCLUDE_DIRS # TODO include - LIBRARIES # TODO -) - if (CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) add_rostest(test/test-color-point-detector.launch) - find_package(jsk_perception) - if(EXISTS ${jsk_perception_DIR}/launch/eusmodel_detection_elevator-panels-eng2.launch OR - EXISTS ${jsk_perception_SOURCE_PREFIX}/launch/eusmodel_detection_elevator-panels-eng2.launch) - add_rostest(test/test-panel-pose-detection.launch ROBOT:=sim) - add_rostest(test/test-button-light.launch ROBOT:=sim) - else() - message(WARNING "${jsk_perception_DIR}/launch/eusmodel_detection_elevator-panels-eng2.launch or") - message(WARNING "${jsk_perception_SOURCE_PREFIX}/launch/eusmodel_detection_elevator-panels-eng2.launch is not found") - message(WARNING "You need to compile jsk_perception to run test-button-light.launch and test-panel-pose-detection.launch") - endif() - # 2 tests below are skipped now since they aren't actually used for testing purpose. - # add_rostest(test/test-modules-callpanel.launch ROBOT:=sim) - # add_rostest(test/test-modules-insidepanel.launch ROBOT:=sim) + add_rostest(test/test-panel-pose-detection.launch ROBOT:=sim) + add_rostest(test/test-button-light.launch ROBOT:=sim) + add_rostest(test/test-modules-callpanel.launch ROBOT:=sim) + add_rostest(test/test-modules-insidepanel.launch ROBOT:=sim) endif() -install(DIRECTORY launch test scripts elevator_numbers +install( + DIRECTORY config elevator_numbers launch node_scripts sample scripts test DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - USE_SOURCE_PERMISSIONS) + USE_SOURCE_PERMISSIONS +) -install(TARGETS color_point_detector - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +install( + TARGETS color_point_detector + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) diff --git a/elevator_move_base_pr2/README.md b/elevator_move_base_pr2/README.md new file mode 100644 index 0000000000..4ad4228b92 --- /dev/null +++ b/elevator_move_base_pr2/README.md @@ -0,0 +1,32 @@ +# elevator_move_base_pr2 + +A ROS package for moving around in the building with elevator by PR2 robot. + +## Usage + +### Go to subway, buy sandwich and come back + +#### Step 1 + +Launch some common nodes on c1 machine to prepare for using elevator. + +```bash +roslaunch elevator_move_base_pr2 elevator_move_base_eng2.launch +``` + +#### Step 2 + +Launch nodes for recognition on GPU machine. +This step will be meld into step 1 in the future. + +```bash +roslaunch elevator_move_base_pr2 fcn_door_button_segmentation.launch +``` + +#### Step 3 + +Launch nodes for application on c1 machine to start demonstration. + +```bash +roslaunch elevator_move_base_pr2 fetch_sandwich.launch +``` diff --git a/elevator_move_base_pr2/apps/apps.installed b/elevator_move_base_pr2/apps/apps.installed new file mode 100644 index 0000000000..051f978779 --- /dev/null +++ b/elevator_move_base_pr2/apps/apps.installed @@ -0,0 +1,3 @@ +apps: + - app: elevator_move_base_pr2/pr2_subway_demo + display: PR2 fetches sandwich from SUBWAY diff --git a/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.app b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.app new file mode 100644 index 0000000000..086b5159cc --- /dev/null +++ b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.app @@ -0,0 +1,6 @@ +display: PR2 fetches sandwich from SUBWAY +description: PR2 fetches sandwich from SUBWAY +platform: pr2 +launch: elevator_move_base_pr2/pr2_subway_demo.xml +interface: elevator_move_base_pr2/pr2_subway_demo.interface +icon: elevator_move_base_pr2/pr2_subway_demo.png diff --git a/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.interface b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.interface new file mode 100644 index 0000000000..044105d644 --- /dev/null +++ b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.interface @@ -0,0 +1,2 @@ +published_topics: {} +subscribed_topics: {} diff --git a/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.png b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.png new file mode 100644 index 0000000000..6b1694d11f Binary files /dev/null and b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.png differ diff --git a/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.xml b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.xml new file mode 100644 index 0000000000..64ea03b156 --- /dev/null +++ b/elevator_move_base_pr2/apps/pr2_subway_demo/pr2_subway_demo.xml @@ -0,0 +1,4 @@ + + + + diff --git a/elevator_move_base_pr2/config/door_button_segmentation_3d.rviz b/elevator_move_base_pr2/config/door_button_segmentation_3d.rviz new file mode 100644 index 0000000000..3c248e8d16 --- /dev/null +++ b/elevator_move_base_pr2/config/door_button_segmentation_3d.rviz @@ -0,0 +1,1359 @@ +Panels: + - Class: rviz/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /ElevatorInfo1 + Splitter Ratio: 0.633333 + Tree Height: 856 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: BaseScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_bellow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_laser_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + br_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + double_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fl_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_l_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_r_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_caster_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_ir_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_kinect_rgb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_kinect_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_mount_prosilica_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_plate_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + high_def_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + high_def_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + l_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + l_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + l_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + laser_tilt_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_tilt_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + narrow_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_child_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + projector_wg6802418_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_elbow_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_cam_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_cam_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_forearm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_l_finger_tip_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_l_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_led_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_accelerometer_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_motor_slider_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_gripper_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_r_finger_tip_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_gripper_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + r_shoulder_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_shoulder_pan_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_torso_lift_side_plate_link: + Alpha: 1 + Show Axes: false + Show Trail: false + r_upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_upper_arm_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_flex_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + r_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_arm_chain_cb_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sensor_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_lift_motor_screw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_l_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + wide_stereo_r_stereo_camera_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Group + Displays: + - Class: jsk_rviz_plugin/PieChart + Enabled: true + Name: Battery0 + Topic: /visualization/battery/value0 + Value: true + auto color change: false + background color: 0; 0; 0 + backround alpha: 0 + foreground alpha: 0.9 + foreground alpha 2: 0.4 + foreground color: 25; 255; 240 + left: 0 + max color: 255; 0; 0 + max value: 100 + min value: 0 + show caption: true + size: 100 + text size: 14 + top: 0 + - Class: jsk_rviz_plugin/PieChart + Enabled: true + Name: Battery1 + Topic: /visualization/battery/value1 + Value: true + auto color change: false + background color: 0; 0; 0 + backround alpha: 0 + foreground alpha: 0.9 + foreground alpha 2: 0.4 + foreground color: 25; 255; 240 + left: 100 + max color: 255; 0; 0 + max value: 100 + min value: 0 + show caption: true + size: 100 + text size: 14 + top: 0 + - Class: jsk_rviz_plugin/PieChart + Enabled: true + Name: Battery2 + Topic: /visualization/battery/value2 + Value: true + auto color change: false + background color: 0; 0; 0 + backround alpha: 0 + foreground alpha: 0.9 + foreground alpha 2: 0.4 + foreground color: 25; 255; 240 + left: 200 + max color: 255; 0; 0 + max value: 100 + min value: 0 + show caption: true + size: 100 + text size: 14 + top: 0 + - Class: jsk_rviz_plugin/PieChart + Enabled: true + Name: Battery3 + Topic: /visualization/battery/value3 + Value: true + auto color change: false + background color: 0; 0; 0 + backround alpha: 0 + foreground alpha: 0.9 + foreground alpha 2: 0.4 + foreground color: 25; 255; 240 + left: 300 + max color: 255; 0; 0 + max value: 100 + min value: 0 + show caption: true + size: 100 + text size: 14 + top: 0 + Enabled: true + Name: Battery + - Class: rviz/Group + Displays: + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Map + Topic: /map + Unreliable: false + Value: false + - Alpha: 0.9 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Eng2/2F + Topic: /eng2/2f + Unreliable: false + Value: true + - Alpha: 0.7 + Class: rviz/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Eng2/7F + Topic: /eng2/7f + Unreliable: false + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /spots_marker_array + Name: SpotMarkers + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Name: Map&Spot + - Class: rviz/Group + Displays: + - Alpha: 0.3 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: GlobalCostmap + Topic: /move_base_node/global_costmap/costmap + Unreliable: false + Value: true + - Alpha: 0.5 + Class: rviz/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: LocalCostmap + Topic: /move_base_node/local_costmap/costmap + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 170; 255; 255 + Enabled: true + Head Diameter: 0.1 + Head Length: 0.05 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: NavFnGlobalPathPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.02 + Shaft Length: 0.1 + Topic: /move_base_node/NavfnROS/plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 0; 255; 0 + Enabled: true + Head Diameter: 0.1 + Head Length: 0.05 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: DWAGlobalPathPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.02 + Shaft Length: 0.1 + Topic: /move_base_node/DWAPlannerROS/global_plan + Unreliable: false + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 255; 85; 0 + Enabled: true + Head Diameter: 0.3 + Head Length: 0.2 + Length: 0.3 + Line Style: Lines + Line Width: 0.03 + Name: DWALocalPathPlan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.03 + Shaft Diameter: 0.1 + Shaft Length: 0.1 + Topic: /move_base_node/DWAPlannerROS/local_plan + Unreliable: false + Value: true + Enabled: true + Name: Navigation + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.303 + Min Value: 0.303 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: BaseScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /base_scan + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + 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: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: KinectCloud + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Points + Topic: /kinect_head/depth_registered/quater/throttled/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Group + Displays: + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: NarrowLeftImage + Topic: /narrow_stereo/left/image_rect_color + Value: true + alpha: 0.8 + height: 120 + keep aspect ratio: false + left: 10 + top: 260 + transport hint: raw + width: 160 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: ElevatorPanel + Topic: /narrow_stereo/left/point_pose_extractor_elevator_panels/debug_image + Value: true + alpha: 0.8 + height: 250 + keep aspect ratio: false + left: 180 + top: 130 + transport hint: raw + width: 160 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: DetectedDoorButton + Topic: /door_button/label_image_decomposer/output/label_viz + Value: true + alpha: 0.8 + height: 120 + keep aspect ratio: false + left: 10 + top: 390 + transport hint: raw + width: 160 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: DetectedCurrentFloor + Topic: /elevator_number/debug_image + Value: true + alpha: 0.8 + height: 120 + keep aspect ratio: false + left: 180 + top: 390 + transport hint: raw + width: 160 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: DetectedLightButton + Topic: /light_detector/debug_image + Value: true + alpha: 0.8 + height: 120 + keep aspect ratio: false + left: 180 + top: 520 + transport hint: raw + width: 160 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: DetectedMirror + Topic: /human_in_mirror/label_image_decomposer/output/label_viz + Value: true + alpha: 0.8 + height: 120 + keep aspect ratio: false + left: 10 + top: 520 + transport hint: raw + width: 160 + - Class: jsk_rviz_plugin/OverlayImage + Enabled: true + Name: DetectedHuman + Topic: /edgetpu_human_pose_estimator/output/image_throttle + Value: true + alpha: 0.8 + height: 120 + keep aspect ratio: false + left: 10 + top: 130 + transport hint: raw + width: 160 + - Class: rviz/TF + Enabled: true + Frame Timeout: 5 + Frames: + All Enabled: false + base_bellow_link: + Value: false + base_footprint: + Value: false + base_laser_link: + Value: false + base_link: + Value: false + bl_caster_l_wheel_link: + Value: false + bl_caster_r_wheel_link: + Value: false + bl_caster_rotation_link: + Value: false + br_caster_l_wheel_link: + Value: false + br_caster_r_wheel_link: + Value: false + br_caster_rotation_link: + Value: false + double_stereo_link: + Value: false + elevator_call_panel: + Value: true + elevator_inside_panel: + Value: true + eng2: + Value: false + eng2/1f: + Value: false + eng2/2f: + Value: false + eng2/3f: + Value: false + eng2/7f: + Value: false + eng2/7f/73B2: + Value: false + eng2/8f: + Value: false + fl_caster_l_wheel_link: + Value: false + fl_caster_r_wheel_link: + Value: false + fl_caster_rotation_link: + Value: false + fr_caster_l_wheel_link: + Value: false + fr_caster_r_wheel_link: + Value: false + fr_caster_rotation_link: + Value: false + head_chain_cb_link: + Value: false + head_mount_kinect_ir_link: + Value: false + head_mount_kinect_ir_optical_frame: + Value: false + head_mount_kinect_rgb_link: + Value: false + head_mount_kinect_rgb_optical_frame: + Value: false + head_mount_link: + Value: false + head_mount_prosilica_link: + Value: false + head_mount_prosilica_optical_frame: + Value: false + head_pan_link: + Value: false + head_plate_frame: + Value: false + head_tilt_link: + Value: false + high_def_frame: + Value: false + high_def_optical_frame: + Value: false + imu_link: + Value: false + l_elbow_flex_link: + Value: false + l_forearm_cam_frame: + Value: false + l_forearm_cam_optical_frame: + Value: false + l_forearm_link: + Value: false + l_forearm_roll_link: + Value: false + l_gripper_l_finger_link: + Value: false + l_gripper_l_finger_tip_frame: + Value: false + l_gripper_l_finger_tip_link: + Value: false + l_gripper_led_frame: + Value: false + l_gripper_motor_accelerometer_link: + Value: false + l_gripper_motor_screw_link: + Value: false + l_gripper_motor_slider_link: + Value: false + l_gripper_palm_link: + Value: false + l_gripper_r_finger_link: + Value: false + l_gripper_r_finger_tip_link: + Value: false + l_gripper_tool_frame: + Value: false + l_shoulder_lift_link: + Value: false + l_shoulder_pan_link: + Value: false + l_torso_lift_side_plate_link: + Value: false + l_upper_arm_link: + Value: false + l_upper_arm_roll_link: + Value: false + l_wrist_flex_link: + Value: false + l_wrist_roll_link: + Value: false + laser_tilt_link: + Value: false + laser_tilt_mount_link: + Value: false + left_arm_chain_cb_link: + Value: false + map: + Value: false + narrow_stereo_l_stereo_camera_frame: + Value: false + narrow_stereo_l_stereo_camera_optical_frame: + Value: false + narrow_stereo_link: + Value: false + narrow_stereo_optical_frame: + Value: false + narrow_stereo_r_stereo_camera_frame: + Value: false + narrow_stereo_r_stereo_camera_optical_frame: + Value: false + odom_combined: + Value: false + projector_wg6802418_child_frame: + Value: false + projector_wg6802418_frame: + Value: false + r_elbow_flex_link: + Value: false + r_forearm_cam_frame: + Value: false + r_forearm_cam_optical_frame: + Value: false + r_forearm_link: + Value: false + r_forearm_roll_link: + Value: false + r_gripper_l_finger_link: + Value: false + r_gripper_l_finger_tip_frame: + Value: false + r_gripper_l_finger_tip_link: + Value: false + r_gripper_led_frame: + Value: false + r_gripper_motor_accelerometer_link: + Value: false + r_gripper_motor_screw_link: + Value: false + r_gripper_motor_slider_link: + Value: false + r_gripper_palm_link: + Value: false + r_gripper_r_finger_link: + Value: false + r_gripper_r_finger_tip_link: + Value: false + r_gripper_tool_frame: + Value: false + r_shoulder_lift_link: + Value: false + r_shoulder_pan_link: + Value: false + r_torso_lift_side_plate_link: + Value: false + r_upper_arm_link: + Value: false + r_upper_arm_roll_link: + Value: false + r_wrist_flex_link: + Value: false + r_wrist_roll_link: + Value: false + right_arm_chain_cb_link: + Value: false + sensor_mount_link: + Value: false + torso_lift_link: + Value: false + torso_lift_motor_screw_link: + Value: false + virtual_camera_frame: + Value: true + wide_stereo_l_stereo_camera_frame: + Value: false + wide_stereo_l_stereo_camera_optical_frame: + Value: false + wide_stereo_link: + Value: false + wide_stereo_optical_frame: + Value: false + wide_stereo_r_stereo_camera_frame: + Value: false + wide_stereo_r_stereo_camera_optical_frame: + Value: false + world: + Value: false + Marker Scale: 1 + Name: ElevatorPanelTF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + world: + eng2: + eng2/1f: + {} + eng2/2f: + {} + eng2/3f: + {} + eng2/7f: + eng2/7f/73B2: + {} + map: + odom_combined: + base_footprint: + base_link: + base_bellow_link: + {} + base_laser_link: + {} + bl_caster_rotation_link: + bl_caster_l_wheel_link: + {} + bl_caster_r_wheel_link: + {} + br_caster_rotation_link: + br_caster_l_wheel_link: + {} + br_caster_r_wheel_link: + {} + fl_caster_rotation_link: + fl_caster_l_wheel_link: + {} + fl_caster_r_wheel_link: + {} + fr_caster_rotation_link: + fr_caster_l_wheel_link: + {} + fr_caster_r_wheel_link: + {} + torso_lift_link: + head_pan_link: + head_tilt_link: + head_chain_cb_link: + {} + head_plate_frame: + head_mount_link: + head_mount_kinect_ir_link: + head_mount_kinect_ir_optical_frame: + {} + head_mount_kinect_rgb_link: + head_mount_kinect_rgb_optical_frame: + {} + head_mount_prosilica_link: + head_mount_prosilica_optical_frame: + {} + projector_wg6802418_frame: + projector_wg6802418_child_frame: + {} + sensor_mount_link: + double_stereo_link: + narrow_stereo_link: + narrow_stereo_l_stereo_camera_frame: + narrow_stereo_l_stereo_camera_optical_frame: + {} + narrow_stereo_r_stereo_camera_frame: + narrow_stereo_r_stereo_camera_optical_frame: + {} + narrow_stereo_optical_frame: + {} + wide_stereo_link: + wide_stereo_l_stereo_camera_frame: + wide_stereo_l_stereo_camera_optical_frame: + {} + wide_stereo_r_stereo_camera_frame: + wide_stereo_r_stereo_camera_optical_frame: + {} + wide_stereo_optical_frame: + {} + high_def_frame: + high_def_optical_frame: + {} + imu_link: + {} + l_shoulder_pan_link: + l_shoulder_lift_link: + l_upper_arm_roll_link: + l_upper_arm_link: + l_elbow_flex_link: + l_forearm_roll_link: + l_forearm_cam_frame: + l_forearm_cam_optical_frame: + {} + l_forearm_link: + l_wrist_flex_link: + l_wrist_roll_link: + l_gripper_palm_link: + l_gripper_l_finger_link: + l_gripper_l_finger_tip_link: + {} + l_gripper_led_frame: + {} + l_gripper_motor_accelerometer_link: + {} + l_gripper_motor_slider_link: + l_gripper_motor_screw_link: + {} + l_gripper_r_finger_link: + l_gripper_r_finger_tip_link: + l_gripper_l_finger_tip_frame: + {} + l_gripper_tool_frame: + {} + left_arm_chain_cb_link: + {} + l_torso_lift_side_plate_link: + {} + laser_tilt_mount_link: + laser_tilt_link: + {} + r_shoulder_pan_link: + r_shoulder_lift_link: + r_upper_arm_roll_link: + r_upper_arm_link: + r_elbow_flex_link: + r_forearm_roll_link: + r_forearm_cam_frame: + r_forearm_cam_optical_frame: + {} + r_forearm_link: + r_wrist_flex_link: + r_wrist_roll_link: + r_gripper_palm_link: + r_gripper_l_finger_link: + r_gripper_l_finger_tip_link: + {} + r_gripper_led_frame: + {} + r_gripper_motor_accelerometer_link: + {} + r_gripper_motor_slider_link: + r_gripper_motor_screw_link: + {} + r_gripper_r_finger_link: + r_gripper_r_finger_tip_link: + r_gripper_l_finger_tip_frame: + {} + r_gripper_tool_frame: + {} + right_arm_chain_cb_link: + {} + r_torso_lift_side_plate_link: + {} + torso_lift_motor_screw_link: + {} + elevator_call_panel: + {} + elevator_inside_panel: + virtual_camera_frame: + {} + eng2/8f: + {} + Update Interval: 0 + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: true + Name: DoorButtonBboxes + Topic: /door_button/cluster_indices_decomposer/boxes + Unreliable: false + Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Auto + line width: 0.005 + only edge: false + show coords: false + - Align Bottom: false + Background Alpha: 0.8 + Background Color: 0; 0; 0 + Class: jsk_rviz_plugin/OverlayText + Enabled: true + Foreground Alpha: 0.8 + Foreground Color: 25; 255; 240 + Name: ROSConsoleText + Overtake Color Properties: true + Overtake Position Properties: true + Topic: /rosconsole_overlay_text/output + Value: true + font: DejaVu Sans Mono + height: 200 + left: 10 + line width: 2 + text size: 8 + top: 650 + width: 800 + Enabled: true + Name: ElevatorInfo + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_footprint + 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 + Topic: /initialpose + - 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: 6.48856 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.5095 + Y: 0.408418 + Z: 1.16486 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.614797 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.76534 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1056 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016a00000399fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000399000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000000ac00000399fc0200000007fb0000001a0057006900640065004c0065006600740049006d0061006700650000000028000003990000000000000000fb0000001e004e006100720072006f0077004c0065006600740049006d00610067006500000003c7fffffffa0000000000000000fb0000001e004e006100720072006f0077004c0065006600740049006d0061006700650100000028000001c90000000000000000fb0000001e0044006f006f00720042007500740074006f006e0049006d0061006700650000000028000003990000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000073f00000159fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f0000003bfc0100000002fb0000000800540069006d006501000000000000073f000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000005cf0000039900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1855 + X: 65 + Y: 24 diff --git a/elevator_move_base_pr2/launch/check_elevator_open.xml b/elevator_move_base_pr2/launch/check_elevator_open.xml index 03c5b96735..8d9e0cc0d4 100644 --- a/elevator_move_base_pr2/launch/check_elevator_open.xml +++ b/elevator_move_base_pr2/launch/check_elevator_open.xml @@ -1,15 +1,38 @@ - - - - - - - - - - - - - + + + + + + + + + + + + filter_limit_min: 1.0 + filter_limit_max: 3.0 + leaf_size: 0.01 + + + + + + + resolution: 0.3 + noise_filter: 10 + + + + diff --git a/elevator_move_base_pr2/launch/door_button_segmentation_3d.launch b/elevator_move_base_pr2/launch/door_button_segmentation_3d.launch new file mode 100644 index 0000000000..4e0c6dddff --- /dev/null +++ b/elevator_move_base_pr2/launch/door_button_segmentation_3d.launch @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + 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/launch/elevator_move_base_eng2.launch b/elevator_move_base_pr2/launch/elevator_move_base_eng2.launch index 7cd4571dd8..ab551609c8 100644 --- a/elevator_move_base_pr2/launch/elevator_move_base_eng2.launch +++ b/elevator_move_base_pr2/launch/elevator_move_base_eng2.launch @@ -8,7 +8,124 @@ - + + + + + + + + + + + + + + default_duration: 0.0 # infinite + + + + + + + + label_names: + - _background_ + - button + bg_label: 0 + approximate_sync: false + queue_size: 100 + + + + + + + + + + + + + + + + + + + + + + + + + default_duration: 0.0 # infinite + + + + + + default_duration: 0.0 # infinite + + + + + + + mirror_label: 1 + score_threshold: 0.3 + no_sync: true + + + + + + + + + queue_size: 300 + + + + + + + update_rate: 3.0 + + + + + + + + + + + diff --git a/elevator_move_base_pr2/launch/elevator_move_base_eng8.launch b/elevator_move_base_pr2/launch/elevator_move_base_eng8.launch index 6f1de48001..3c36c6a3d3 100644 --- a/elevator_move_base_pr2/launch/elevator_move_base_eng8.launch +++ b/elevator_move_base_pr2/launch/elevator_move_base_eng8.launch @@ -9,7 +9,6 @@ - diff --git a/elevator_move_base_pr2/launch/elevator_move_base_modules.xml b/elevator_move_base_pr2/launch/elevator_move_base_modules.xml index 91bee100c6..4372921ff8 100644 --- a/elevator_move_base_pr2/launch/elevator_move_base_modules.xml +++ b/elevator_move_base_pr2/launch/elevator_move_base_modules.xml @@ -1,44 +1,102 @@ - + + - + + + - - + + + + + update_rate: 2.0 + + + + + + + + + + + - - - - - - + + + + + + + default_duration: 0.0 # infinite + + + + + + + red: 253 + green: 251 + blue: 240 + decay_r: 0.1 + decay_g: 0.1 + decay_b: 0.05 + - - - + + + + + + + default_duration: 0.0 # infinite + + + + + - - + + + + show_probability: false + - - - - - + - + + diff --git a/elevator_move_base_pr2/launch/elevator_panels_detection_eng2.launch b/elevator_move_base_pr2/launch/elevator_panels_detection_eng2.launch new file mode 100644 index 0000000000..81133c1e80 --- /dev/null +++ b/elevator_move_base_pr2/launch/elevator_panels_detection_eng2.launch @@ -0,0 +1,71 @@ + + + + + + + + + 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 + + + + + + + default_duration: 0.0 # infinite + + + + + + + + + + template_filename: $(find jsk_perception)/template/elevator_inside_panel_eng2.png + child_frame_id: matching + object_width: 0.363 + object_height: 0.294 + reprojection_threshold: 1.0 + distanceratio_threshold: 0.8 + error_threshold: 70.0 + theta_step: 5.0 + phi_step: 5.0 + relative_pose: "0.147 0.1815 -0.0005 0.0 -0.707107 0.0 0.707107" + viewer_window: false + window_name: elevator_inside_panel + + + diff --git a/elevator_move_base_pr2/launch/elevator_panels_detection_eng8.launch b/elevator_move_base_pr2/launch/elevator_panels_detection_eng8.launch new file mode 100644 index 0000000000..3418725b95 --- /dev/null +++ b/elevator_move_base_pr2/launch/elevator_panels_detection_eng8.launch @@ -0,0 +1,63 @@ + + + + + + + + + default_duration: 0.0 # infinite + + + + + + + + + + template_filename: $(find jsk_perception)/template/elevator_call_panel_eng8.png + child_frame_id: matching + object_width: 0.276 + object_height: 0.285 + error_threshold: 100.0 + relative_pose: "0.1425 0.138 -0.0005 0.0 -0.707107 0.0 0.707107" + viewer_window: false + window_name: elevator_call_panel + + + + + + + default_duration: 0.0 # infinite + + + + + + + + + + template_filename: $(find jsk_perception)/template/elevator_inside_panel_eng8.png + child_frame_id: matching + object_width: 0.47 + object_height: 0.35 + error_threshold: 100.0 + relative_pose: "0.175 0.235 -0.0005 0.0 -0.707107 0.0 0.707107" + viewer_window: false + window_name: elevator_inside_panel + + + diff --git a/elevator_move_base_pr2/launch/extract_eng2_forum_door_point_cloud.launch b/elevator_move_base_pr2/launch/extract_eng2_forum_door_point_cloud.launch new file mode 100644 index 0000000000..0c3be94373 --- /dev/null +++ b/elevator_move_base_pr2/launch/extract_eng2_forum_door_point_cloud.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + frame_id: /eng2/2f + boxes: + - position: [10.5, -27.0, 1.0] + rotation: [0.0, 0.0, 0.0] + dimension: [0.6, 1.4, 1.8] + label: 0 + rate: 10.0 + + + + + + + index: 0 + + + + + + + + + + + + + + keep_organized: false + approximate_sync: true + max_queue_size: 100 + + + + + + diff --git a/elevator_move_base_pr2/launch/fcn_door_button_segmentation.launch b/elevator_move_base_pr2/launch/fcn_door_button_segmentation.launch new file mode 100644 index 0000000000..8edb13f37d --- /dev/null +++ b/elevator_move_base_pr2/launch/fcn_door_button_segmentation.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + use_mask: false + gpu: $(arg GPU) + model_name: fcn8s_at_once + model_file: $(arg MODEL_FILE) + target_names: + - _background_ + - button + bg_label: 0 + proba_threshold: 0.0 + + + + + + + + + + + + + + + + + + + + + + diff --git a/elevator_move_base_pr2/launch/find_human_in_mirror.launch b/elevator_move_base_pr2/launch/find_human_in_mirror.launch new file mode 100644 index 0000000000..f5690c2ca4 --- /dev/null +++ b/elevator_move_base_pr2/launch/find_human_in_mirror.launch @@ -0,0 +1,95 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + model_name: fcn8s_depth_prediction_concat_first + model_file: $(arg MODEL_FILE) + gpu: $(arg GPU) + queue_size: 10 + approximate_sync: true + slop: 0.1 + bg_label: 0 + proba_threshold: 0.5 + target_names: + - _background_ + - mirror + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/elevator_move_base_pr2/launch/save_images_from_pr2_head_cameras.launch b/elevator_move_base_pr2/launch/save_images_from_pr2_head_cameras.launch new file mode 100644 index 0000000000..8c1d3098e1 --- /dev/null +++ b/elevator_move_base_pr2/launch/save_images_from_pr2_head_cameras.launch @@ -0,0 +1,72 @@ + + + + + + + + + + + save_dir: $(arg save_dir) + topics: + - name: /kinect_head/rgb/image_rect_color + msg_class: sensor_msgs/Image + fname: kinect_rgb.jpg + savetype: ColorImage + - name: /narrow_stereo/left/image_rect_color + msg_class: sensor_msgs/Image + fname: narrow_left.jpg + savetype: ColorImage + - name: /narrow_stereo/right/image_rect_color + msg_class: sensor_msgs/Image + fname: narrow_right.jpg + savetype: ColorImage + - name: /wide_stereo/left/image_rect_color + msg_class: sensor_msgs/Image + fname: wide_left.jpg + savetype: ColorImage + - name: /wide_stereo/right/image_rect_color + msg_class: sensor_msgs/Image + fname: wide_right.jpg + savetype: ColorImage + approximate_sync: true + slop: 1.0 + + + + + + + + + + + + + + input_topics: + - /kinect_head/rgb/image_rect_color + - /narrow_stereo/left/image_rect_color + - /narrow_stereo/right/image_rect_color + - /wide_stereo/left/image_rect_color + - /wide_stereo/right/image_rect_color + no_sync: true + draw_topic_name: true + font_scale: 0.8 + + + + + + + + diff --git a/elevator_move_base_pr2/launch/ssd.launch b/elevator_move_base_pr2/launch/ssd.launch new file mode 100644 index 0000000000..e6d0d12068 --- /dev/null +++ b/elevator_move_base_pr2/launch/ssd.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + gpu: $(arg gpu) + model_path: voc0712 + + + + + + + + + approximate_sync: true + queue_size: 10 + slop: 0.1 + + + + + + + + + + + diff --git a/elevator_move_base_pr2/launch/template-eng2.yaml b/elevator_move_base_pr2/launch/template-eng2.yaml index d30e354fce..fee3de9a16 100644 --- a/elevator_move_base_pr2/launch/template-eng2.yaml +++ b/elevator_move_base_pr2/launch/template-eng2.yaml @@ -1,19 +1,56 @@ -#template_list: B1F 1F 2F 3F 4F 5F 6F 7F 8F 9F 10F 11F 12F -template_list: 2F 7F 8F +method: CCOEFF +template_list: B1F 1F 2F 3F 4F 5F 6F 7F 8F 9F 10F 11F 12F +#template_list: 2F 7F 8F template: + B1F: + name: /b1f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/b1f.png" + thre: 0.75 1F: name: /1f path: "package://elevator_move_base_pr2/elevator_numbers/eng2/1f.png" - thre: 0.11 + thre: 0.75 2F: name: /2f path: "package://elevator_move_base_pr2/elevator_numbers/eng2/2f.png" - thre: 0.11 + thre: 0.75 + 3F: + name: /3f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/3f.png" + thre: 0.75 + 4F: + name: /4f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/4f.png" + thre: 0.75 + 5F: + name: /5f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/5f.png" + thre: 0.75 + 6F: + name: /6f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/6f.png" + thre: 0.75 7F: name: /7f path: "package://elevator_move_base_pr2/elevator_numbers/eng2/7f.png" - thre: 0.11 + thre: 0.75 8F: name: /8f path: "package://elevator_move_base_pr2/elevator_numbers/eng2/8f.png" - thre: 0.11 + thre: 0.75 + 9F: + name: /9f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/9f.png" + thre: 0.75 + 10F: + name: /10f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/10f.png" + thre: 0.75 + 11F: + name: /11f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/11f.png" + thre: 0.75 + 12F: + name: /12f + path: "package://elevator_move_base_pr2/elevator_numbers/eng2/12f.png" + thre: 0.75 diff --git a/elevator_move_base_pr2/msg/Float32Stamped.msg b/elevator_move_base_pr2/msg/Float32Stamped.msg deleted file mode 100644 index 090755bb1d..0000000000 --- a/elevator_move_base_pr2/msg/Float32Stamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -float32 data \ No newline at end of file diff --git a/elevator_move_base_pr2/node_scripts/bounding_box_centroid_3d.py b/elevator_move_base_pr2/node_scripts/bounding_box_centroid_3d.py new file mode 100755 index 0000000000..bfd3adfa43 --- /dev/null +++ b/elevator_move_base_pr2/node_scripts/bounding_box_centroid_3d.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# Author: Yuki Furuta + +import numpy as np +from cv_bridge import CvBridge +import rospy +import message_filters +from jsk_topic_tools import ConnectionBasedTransport + +from jsk_recognition_msgs.msg import RectArray +from sensor_msgs.msg import Image +from sensor_msgs.msg import CameraInfo +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseArray + + +class BoundingBoxCentroid3D(ConnectionBasedTransport): + def __init__(self): + super(BoundingBoxCentroid3D, self).__init__() + self.approximate_sync = rospy.get_param("~approximate_sync", True) + self.queue_size = rospy.get_param("~queue_size", 10) + self.slop = rospy.get_param("~slop", 0.1) + self.buff_size = rospy.get_param("~buff_size", 2**28) + self.cv_bridge = CvBridge() + self.camera_info_msg = None + self.pub_pose = self.advertise("~output", PoseArray, queue_size=1) + + def subscribe(self): + self.subscribers = [ + message_filters.Subscriber( + "~input", RectArray, + queue_size=1, buff_size=self.buff_size), + message_filters.Subscriber( + "~input/depth", Image, + queue_size=1, buff_size=self.buff_size), + ] + if self.approximate_sync: + sync = message_filters.ApproximateTimeSynchronizer( + self.subscribers, queue_size=self.queue_size, slop=self.slop) + else: + sync = message_filters.TimeSynchronizer( + self.subscribers, queue_size=self.queue_size) + sync.registerCallback(self.callback) + + self.sub_cam_info = rospy.Subscriber( + "~input/camera_info", CameraInfo, self.cam_cb, + queue_size=1, buff_size=self.buff_size) + + def unsubscribe(self): + for sub in self.subscribers: + sub.unregister() + self.sub_cam_info.unregister() + + def cam_cb(self, msg): + self.camera_info_msg = msg + self.sub_cam_info.unregister() + + def callback(self, rects, depth_msg): + if self.camera_info_msg is None: + rospy.logwarn("Camera info is not yet received! Please publish camera info first!") + return + + rospy.logdebug("callback: %s sec delayed" % (rospy.Time.now() - rects.header.stamp).to_sec()) + + try: + depth = self.cv_bridge.imgmsg_to_cv2(depth_msg, "passthrough") + except: + rospy.logerr("Failed to convert depth image") + return + + if depth_msg.encoding == "16UC1": + depth = np.asarray(depth, dtype=np.float32) + depth /= 1000.0 + elif depth_msg.encoding != "32FC1": + rospy.logerr("Unsupported depth image encoding: %s" % depth_msg.encoding) + return + + fx = self.camera_info_msg.K[0] + fy = self.camera_info_msg.K[4] + cx = self.camera_info_msg.K[2] + cy = self.camera_info_msg.K[5] + + msg = PoseArray(header=rects.header) + for r in rects.rects: + x = r.x + r.width / 2.0 + y = r.y + r.height / 2.0 + z = float(depth[int(np.round(y))][int(np.round(x))]) + if np.isnan(z): + z = 0.0 + x = (x - cx) * z / fx + y = (y - cy) * z / fy + + p = Pose() + p.position.x = x + p.position.y = y + p.position.z = z + p.orientation.w = 1 + msg.poses.append(p) + + self.pub_pose.publish(msg) + + +if __name__ == '__main__': + rospy.init_node("bounding_box_centroid_3d") + bbc3d = BoundingBoxCentroid3D() + rospy.spin() diff --git a/elevator_move_base_pr2/node_scripts/concat_pose_and_class.py b/elevator_move_base_pr2/node_scripts/concat_pose_and_class.py new file mode 100755 index 0000000000..012f68a68e --- /dev/null +++ b/elevator_move_base_pr2/node_scripts/concat_pose_and_class.py @@ -0,0 +1,63 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import rospy +import message_filters +from geometry_msgs.msg import PoseArray +from jsk_topic_tools import ConnectionBasedTransport +from jsk_recognition_msgs.msg import ClassificationResult +from posedetection_msgs.msg import ObjectDetection +from posedetection_msgs.msg import Object6DPose + + +class ConcatPoseAndClass(ConnectionBasedTransport): + + def __init__(self): + super(ConcatPoseAndClass, self).__init__() + self.frame_id = rospy.get_param('~frame_id', "base_footprint") + self.pub_detect = self.advertise( + "~output", ObjectDetection, queue_size=1) + + def subscribe(self): + queue_size = rospy.get_param("~queue_size", 100) + approximate_sync = rospy.get_param('~approximate_sync', False) + self.subscribers = [ + message_filters.Subscriber("~input/poses", PoseArray), + message_filters.Subscriber("~input/classes", ClassificationResult), + ] + if approximate_sync: + slop = rospy.get_param('~slop', 0.1) + sync = message_filters.ApproximateTimeSynchronizer( + self.subscribers, queue_size=queue_size, slop=slop) + else: + sync = message_filters.TimeSynchronizer( + self.subscribers, queue_size) + sync.registerCallback(self.cb) + + def unsubscribe(self): + for s in self.subscribers: + s.unregister() + + def cb(self, poses, classes): + msg = ObjectDetection() + msg.header = poses.header + for pose, label, prob in zip( + poses.poses, + classes.label_names, + classes.label_proba): + if not label: + continue + + object_pose = Object6DPose() + object_pose.pose = pose + msg.header.frame_id = self.frame_id + object_pose.reliability = prob + object_pose.type = label + msg.objects.append(object_pose) + self.pub_detect.publish(msg) + + +if __name__ == '__main__': + rospy.init_node("concat_pose_and_class") + detector = ConcatPoseAndClass() + rospy.spin() diff --git a/elevator_move_base_pr2/node_scripts/find_human_in_mirror.py b/elevator_move_base_pr2/node_scripts/find_human_in_mirror.py new file mode 100755 index 0000000000..fd4f45431c --- /dev/null +++ b/elevator_move_base_pr2/node_scripts/find_human_in_mirror.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python + +import cv_bridge +from jsk_recognition_msgs.msg import BoolStamped +from jsk_recognition_msgs.msg import PeoplePoseArray +from jsk_topic_tools import ConnectionBasedTransport +import message_filters +import numpy as np +import rospy +from sensor_msgs.msg import Image + + +class FindHumanInMirror(ConnectionBasedTransport): + + def __init__(self): + super(self.__class__, self).__init__() + self.mirror_label = rospy.get_param('~mirror_label', 1) + self.score_thre = rospy.get_param('~score_threshold', 0.5) + self.bridge = cv_bridge.CvBridge() + self.no_sync = rospy.get_param('~no_sync', False) + self.approximate_sync = rospy.get_param('~approximate_sync', False) + self.ppl_msg = None + self.pub_inside = self.advertise( + '~output/inside_mirror', BoolStamped, queue_size=1) + self.pub_outside = self.advertise( + '~output/outside_mirror', BoolStamped, queue_size=1) + + def subscribe(self): + if self.no_sync: + sub_people_pose = rospy.Subscriber( + '~input/people_pose_array', PeoplePoseArray, + self._people_pose_cb, queue_size=1) + sub_label = rospy.Subscriber( + '~input/label', Image, self._label_cb, queue_size=1) + else: + sub_people_pose = message_filters.Subscriber( + '~input/people_pose_array', PeoplePoseArray, queue_size=1) + sub_label = message_filters.Subscriber( + '~input/label', Image, + queue_size=1, buff_size=2**24) + if self.approximate_sync: + queue_size = rospy.get_param('~queue_size', 10) + slop = rospy.get_param('~slop', 0.1) + sync = message_filters.ApproximateTimeSynchronizer( + fs=self.subs, queue_size=queue_size, slop=slop) + else: + sync = message_filters.TimeSynchronizer( + fs=self.subs, queue_size=queue_size) + sync.registerCallback(self._sync_cb) + self.subs = [sub_people_pose, sub_label] + + def unsubscribe(self): + for sub in self.subs: + sub.unregister() + + def _people_pose_cb(self, ppl_msg): + self.ppl_msg = ppl_msg + + def _label_cb(self, lbl_msg): + if self.ppl_msg is not None: + return self._cb(self.ppl_msg, lbl_msg) + + def _cb(self, ppl_msg, lbl_msg): + inside_msg = BoolStamped(header=lbl_msg.header, data=False) + outside_msg = BoolStamped(header=lbl_msg.header, data=False) + if not ppl_msg.poses: + rospy.loginfo('No human found.') + self.pub_inside.publish(inside_msg) + self.pub_outside.publish(outside_msg) + return + + lbl = self.bridge.imgmsg_to_cv2(lbl_msg) + for person in ppl_msg.poses: + # mean of [each_limb_score * int(bool(each_limb_is_in_mirror))] + inside_score = np.mean([ + int((0 <= limb.position.x <= lbl_msg.width) and + (0 <= limb.position.y <= lbl_msg.height) and + lbl[int(limb.position.y), + int(limb.position.x)] == self.mirror_label) * + person.scores[i] + for i, limb in enumerate(person.poses) + ]) + # mean of [each_limb_score * int(bool(each_limb_is_in_background))] + outside_score = np.mean([ + int((0 <= limb.position.x <= lbl_msg.width) and + (0 <= limb.position.y <= lbl_msg.height) and + lbl[int(limb.position.y), + int(limb.position.x)] != self.mirror_label) * + person.scores[i] + for i, limb in enumerate(person.poses) + ]) + rospy.loginfo( + 'inside score: {}, outside score: {}, threshold: {}'.format( + inside_score, outside_score, self.score_thre)) + if inside_score >= max(self.score_thre, outside_score): + inside_msg.data = True + if outside_score >= max(self.score_thre, inside_score): + outside_msg.data = True + + self.pub_inside.publish(inside_msg) + self.pub_outside.publish(outside_msg) + return + + +if __name__ == '__main__': + rospy.init_node('find_human_in_mirror') + app = FindHumanInMirror() + rospy.spin() diff --git a/elevator_move_base_pr2/node_scripts/look-at-human.l b/elevator_move_base_pr2/node_scripts/look-at-human.l new file mode 100755 index 0000000000..f324afed03 --- /dev/null +++ b/elevator_move_base_pr2/node_scripts/look-at-human.l @@ -0,0 +1,87 @@ +#!/usr/bin/env roseus + +(ros::roseus-add-msgs "jsk_recognition_msgs") +(ros::roseus-add-srvs "std_srvs") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") + + +(defvar *look-at-human-enabled* nil) +;; (defvar *pr2*) +;; (defvar *ri*) + + +(defun people-pose-array-cb (msg) + (unless (send msg :poses) + (return-from people-pose-array-cb nil)) + + (let (limb-names limb-scores limb-poses + (target-limbs '("nose" "left eye" "left ear" "right eye" "right ear")) + person-count limb-count (max-score 0) + (look-person-idx nil) (look-limb-idx nil) + look-person look-uv look-xyz) + + ;; Search highest score for each person + (setq person-count 0) + (dolist (person (send msg :poses)) + (setq limb-names (send person :limb_names)) + (setq limb-scores (send person :scores)) + (setq limb-poses (send person :poses)) + + ;; For each limb + (setq limb-count 0) + (dolist (limb limb-names) + (dolist (target target-limbs) + (when (and (equal (string-downcase limb) target) + (> (elt limb-scores limb-count) max-score)) + (setq max-score (elt limb-scores limb-count)) + (setq look-person-idx person-count) + (setq look-limb-idx limb-count))) + (incf limb-count)) + (incf person-count)) + + ;; Do nothing when no valid limb was found + (when (or (null look-person-idx) (null look-limb-idx)) + (return-from people-pose-array-cb nil)) + (setq look-person (elt (send msg :poses) look-person-idx)) + (when (< (elt (send look-person :scores) look-limb-idx) 0.5) + (return-from people-pose-array-cb nil)) + + ;; Look at target + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (setq look-uv + (send (elt (send look-person :poses) look-limb-idx) :position)) + (setq look-xyz (scale 1e+6 (send (send *pr2* :kinect_head-rgb) + :ray (send look-uv :x) (send look-uv :y)))) + (send *pr2* :head :look-at look-xyz) + (send *ri* :angle-vector (send *pr2* :angle-vector) 700 :head-controller))) + +(defun start-look-at-human (req) + (setq *look-at-human-enabled* t) + (ros::subscribe + "~input/people_pose_array" jsk_recognition_msgs::PeoplePoseArray + #'people-pose-array-cb) + (instance std_srvs::EmptyResponse :init)) + +(defun stop-look-at-human (req) + (setq *look-at-human-enabled* nil) + (ros::unsubscribe "~input/people_pose_array") + (instance std_srvs::EmptyResponse :init)) + + +;; Main process +(ros::roseus "look_at_human") + +(pr2-init) + +(ros::advertise-service "~start" std_srvs::Empty #'start-look-at-human) +(ros::advertise-service "~stop" std_srvs::Empty #'stop-look-at-human) +(ros::advertise "~enabled" std_msgs::Bool 1) +(unix:usleep (* 100 1000)) + +(ros::rate 2) +(while (ros::ok) + (ros::spin-once) + (ros::publish "~enabled" + (instance std_msgs::Bool :init :data *look-at-human-enabled*)) + (ros::sleep)) diff --git a/elevator_move_base_pr2/node_scripts/match_template.py b/elevator_move_base_pr2/node_scripts/match_template.py new file mode 100755 index 0000000000..3ea27e6255 --- /dev/null +++ b/elevator_move_base_pr2/node_scripts/match_template.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# Author: + +from collections import namedtuple +from urlparse import urlparse + +import cv2 +import cv_bridge +from jsk_topic_tools import ConnectionBasedTransport +import matplotlib +matplotlib.use('Agg') # NOQA +import matplotlib.pyplot as plt +import numpy as np +from roseus.msg import StringStamped +import rospkg +import rospy +from sensor_msgs.msg import Image + + +Template = namedtuple('Template', ['name', 'image', 'thre', 'method']) +Result = namedtuple('Result', ['score', 'found', 'top_left']) + + +class MatchTemplate(ConnectionBasedTransport): + def __init__(self): + super(MatchTemplate, self).__init__() + self.rospack = rospkg.RosPack() + self.cv_bridge = cv_bridge.CvBridge() + self.pub_result = self.advertise( + '~result', StringStamped, queue_size=1) + self.pub_debug = self.advertise( + '~debug_image', Image, queue_size=1) + self.show_proba = rospy.get_param('~show_probability', True) + self.templates = self.load_templates() + rospy.loginfo('Initialized with %d templates' % len(self.templates)) + + def subscribe(self): + self.sub = rospy.Subscriber( + '~image', Image, self.image_cb, queue_size=1) + + def unsubscribe(self): + self.sub.unregister() + + def load_templates(self): + method = rospy.get_param('~method', '') + if method == 'CCORR': + method = cv2.TM_CCORR_NORMED + elif method == 'CCOEFF': + method = cv2.TM_CCOEFF_NORMED + elif method == 'SQDIFF': + method = cv2.TM_SQDIFF_NORMED + else: + method = cv2.TM_SQDIFF_NORMED + + template_list = rospy.get_param('~template_list').split() + templates = {} + for typename in template_list: + prefix = '~template/' + typename + imgpath = self.resolve_ros_path(rospy.get_param(prefix + '/path')) + + templates[typename] = Template( + name=rospy.get_param(prefix + '/name'), + thre=rospy.get_param(prefix + '/thre'), + method=method, + image=cv2.imread(imgpath, cv2.IMREAD_GRAYSCALE), + ) + return templates + + def resolve_ros_path(self, path): + path = path.strip() + if path.startswith('package://'): + uri = urlparse(path) + pkgdir = self.rospack.get_path(uri.hostname) + return pkgdir + uri.path + else: + return path + + def image_cb(self, msg): + try: + img = self.cv_bridge.imgmsg_to_cv2(msg, 'mono8') + except cv_bridge.CvBridgeError as e: + rospy.logerr(e) + return + + results = dict() + for typename, template in sorted(self.templates.items()): + res = cv2.matchTemplate(img, template.image, template.method) + min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res) + + if template.method == cv2.TM_SQDIFF_NORMED: + score = min_val + result = Result( + score=score, + found=min_val < template.thre, + top_left=min_loc) + else: + score = max_val + result = Result( + score=score, + found=max_val > template.thre, + top_left=max_loc) + results[template.name] = result + + # publish result + msg = StringStamped(header=msg.header) + msg.data = ' '.join([n for n, r in results.items() if r.found]) + rospy.loginfo('Matched template: {}'.format(msg.data)) + self.pub_result.publish(msg) + + # publish debug image + self.publish_debug(img, results) + + def publish_debug(self, img, results): + templates = self.templates.values() + templates.sort(key=lambda t: t.name) + imgs = [t.image for t in templates] + + tmpl_img = np.hstack(imgs) + scale = 1.0 * tmpl_img.shape[1] / img.shape[1] + if scale >= 1.0: + img_scale = scale + tmpl_scale = 1.0 + img = cv2.resize(img, None, + fx=img_scale, fy=img_scale) + else: + tmpl_scale = 1.0 / scale + img_scale = 1.0 + tmpl_img = cv2.resize(tmpl_img, None, + fx=tmpl_scale, fy=tmpl_scale) + + debug = np.vstack((tmpl_img, img)) + debug = cv2.cvtColor(debug, cv2.COLOR_GRAY2BGR) + + for i, t in enumerate(templates): + res = results[t.name] + if res.found: + w = int(t.image.shape[1] * tmpl_scale) + h = int(t.image.shape[0] * tmpl_scale) + top_left = (i * w, 0) + bottom_right = ((i + 1) * w, h) + cv2.rectangle(debug, top_left, bottom_right, (0, 0, 255), 3) + + w = int(t.image.shape[1] * img_scale) + h = int(t.image.shape[0] * img_scale) + top_left = ( + int(res.top_left[0] * img_scale), + int(res.top_left[1] * img_scale + tmpl_img.shape[0]) + ) + bottom_right = (top_left[0] + w, top_left[1] + h) + cv2.rectangle(debug, top_left, bottom_right, (0, 0, 255), 3) + + if self.show_proba: + fig = plt.figure(dpi=200) + ax = fig.add_subplot(1, 1, 1) + ax.set_axis_off() + ax.imshow(debug) + + # draw roi if found + for i, t in enumerate(templates): + res = results[t.name] + w = t.image.shape[1] * tmpl_scale + h = t.image.shape[0] * tmpl_scale + ax.text(i * w + 20, h + 50, + '%s:\n%.2f' % (t.name, res.score), + fontsize=8, + bbox={'facecolor': 'white', + 'alpha': 0.5, + 'pad': 3}) + + plt.margins(0, 0) + plt.subplots_adjust( + left=0, right=1, top=1, bottom=0, hspace=0, wspace=0) + fig = plt.gcf() + fig.canvas.draw() + w, h = fig.canvas.get_width_height() + debug = np.fromstring(fig.canvas.tostring_rgb(), dtype=np.uint8) + debug.shape = (h, w, 3) + fig.clf() + plt.close() + + try: + msg = self.cv_bridge.cv2_to_imgmsg(debug, 'bgr8') + self.pub_debug.publish(msg) + except cv_bridge.CvBridgeError as e: + rospy.logerr(e) + + +if __name__ == '__main__': + rospy.init_node('match_template') + n = MatchTemplate() + rospy.spin() diff --git a/elevator_move_base_pr2/node_scripts/save_image_by_btn.py b/elevator_move_base_pr2/node_scripts/save_image_by_btn.py new file mode 100755 index 0000000000..4f87656ebe --- /dev/null +++ b/elevator_move_base_pr2/node_scripts/save_image_by_btn.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python + +from jsk_gui_msgs.srv import YesNo +import rospy +from std_srvs.srv import Trigger + + +def main(): + rospy.wait_for_service('rqt_yn_btn', timeout=None) + rospy.wait_for_service('data_collection_server/save_request', timeout=None) + rqt_yn_btn = rospy.ServiceProxy('rqt_yn_btn', YesNo) + save_req = rospy.ServiceProxy( + 'data_collection_server/save_request', Trigger) + + while not rospy.is_shutdown(): + can_save = False + while can_save is False or can_save.yes is False: + try: + can_save = rqt_yn_btn('Press [Yes] to save image.') + except rospy.ServiceException as e: + rospy.logerr('Service call failed. [{}]'.format(e)) + save_res = save_req() + rospy.loginfo('success: {}, message: {}'.format( + save_res.success, save_res.message)) + + +if __name__ == '__main__': + rospy.init_node('save_image_by_btn') + main() + rospy.spin() diff --git a/elevator_move_base_pr2/package.xml b/elevator_move_base_pr2/package.xml index 22d4363637..4d8ec0a3dc 100644 --- a/elevator_move_base_pr2/package.xml +++ b/elevator_move_base_pr2/package.xml @@ -13,30 +13,53 @@ catkin - roseus - rospy - roscpp - pr2eus - roseus_smach - jsk_maps - jsk_perception - move_base_msgs - sound_play - std_srvs - std_msgs - nav_msgs + cv_bridge + geometry_msgs image_geometry image_transport - cv_bridge - message_generation - message_runtime - + roscpp + std_msgs + tf + app_manager + cv_bridge + geometry_msgs + image_transport + image_view + imagesift + jsk_data + jsk_gui_msgs + jsk_maps + jsk_pcl_ros + jsk_pcl_ros_utils + jsk_perception + jsk_recognition_utils + jsk_rqt_plugins + jsk_rviz_plugins + jsk_tools + jsk_topic_tools + move_base_msgs + nav_msgs + nodelet + pr2_machine + pr2eus + roseus + roseus_smach + rospy + rviz + sound_play + std_msgs + std_srvs + topic_tools + + + image_proc + rosbag rostest - - - + + + diff --git a/elevator_move_base_pr2/sample/fetch-sandwich.l b/elevator_move_base_pr2/sample/fetch-sandwich.l new file mode 100755 index 0000000000..ebd9bb62b7 --- /dev/null +++ b/elevator_move_base_pr2/sample/fetch-sandwich.l @@ -0,0 +1,371 @@ +#!/usr/bin/env roseus +;; -*- coding: utf-8 -*- +;; fetch-sandwich.l +;; Author: furushchev + +(ros::roseus-add-msgs "jsk_recognition_msgs") +(ros::roseus-add-msgs "sensor_msgs") +(ros::roseus-add-srvs "std_srvs") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") +(require :app-utils "package://jsk_demo_common/euslisp/app-utils.l") +(require :pr2-action "package://jsk_demo_common/euslisp/pr2-action.l") +(require :eng2-scene "package://jsk_maps/src/eng2-scene.l") + +(load "package://elevator_move_base_pr2/src/utils.l") + + +(defvar *initial-pose*) +(defvar *pr2*) +(defvar *ri*) +(defvar *scene*) + + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; PR2 Utility Functions +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + +(defun look-front () + (send *pr2* :head :angle-vector #f(0 0)) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation)) + + +(defun pr2-pick-bag-pose () + #| + (send *pr2* :reset-pose) + (send *pr2* :torso :waist-z :joint-angle 325.0) + (let ((cds (make-cascoords :pos (float-vector 800 0 1300)))) + (send cds :rotate pi/2 :x) + (send *pr2* :rarm :inverse-kinematics + cds + :use-torso t + :look-at-target t)) + |# + (float-vector 325.0 60.0 74.0 70.0 -120.0 20.0 -30.0 180.0 -13.5232 -15.8808 + -82.6112 -83.1698 -162.008 -72.0918 170.603 0.0 40.2852)) + + +(defun wait-for-hand-over (arm &optional (timeout 10) (n-trial 3)) + (dotimes (i n-trial) + (when (wait-for-hand-impact arm :timeout timeout) + (ros::ros-info "Received sandwich.") + (speak-jp "ありがとうございます。" :wait t) + (return-from wait-for-hand-over t)) + (ros::ros-warn + (format nil "Hand impact timed out (~A sec) without response" timeout)) + (speak-jp "袋を私の手に掛けて、手を揺らしてください。" :wait t)) + ;; fail + (speak-jp "あれ、サンドイッチ。。。" :wait t) + (return-from wait-for-hand-over nil)) + + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;;; Actions +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + + +(defun init () + (setq *pr2* (pr2)) + (setq *ri* (instance pr2-interface :init + :move-base-action-name "elevator_move_base")) + (send *pr2* :reset-pose) + (pr2-tuckarm-pose) + (look-front) + (setq *scene* (make-eng2-scene)) + (setq *initial-pose* (send *ri* :state :worldcoords))) + + +(defun go-to-forum-door-inside () + (clear-costmap) + ;; XXX: Avoid getting stuck on braille block (bumpy tiles for the blind) + ;; at elevator hall on 2F. + (send *ri* :move-to + (send (send (send *scene* :spot "/eng2/2f/forum-door-inside") + :copy-worldcoords) :translate #f(-3200 -100 0) :world)) + (send *ri* :go-pos-unsafe 3.2 0 0) + ;; Modify position + (send *ri* :move-to (send *scene* :spot "/eng2/2f/forum-door-inside")) + (update-robot-position)) + + +(defun check-forum-door-open (&optional (max-cloud-size 500) (timeout 10000)) + (let (is-door-open cloud-msg) + (send *pr2* :head :look-at + (send (send *scene* :spot "/eng2/2f/forum-door-button") :worldpos)) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :wait-interpolation) + (ros::ros-info "Checking if door in front of forum is open...") + ;; Assume the point cloud to be unorganized and is_dense: True. + (setq cloud-msg + (one-shot-subscribe + "extract_eng2_forum_door_point_cloud/extract_indices/output" + sensor_msgs::PointCloud2 + :timeout timeout ;; timeout[msec] + :after-stamp (ros::time+ (ros::time-now) (ros::time 1.0)))) + (unless cloud-msg + (ros::ros-warn + (format nil "No point cloud was received in ~A[msec]." timeout)) + (return-from check-forum-door-open nil)) + (setq is-door-open + (<= (* (send cloud-msg :width) (send cloud-msg :height)) + max-cloud-size)) + (ros::ros-info + (format nil "number of points: ~A, max-cloud-size: ~A, is-door-open: ~A" + (* (send cloud-msg :width) (send cloud-msg :height)) + max-cloud-size + is-door-open)) + (when is-door-open + (ros::ros-info "Door in front of forum is open.") + (speak-jp "ドアが開いています。") + (return-from check-forum-door-open t)) + (ros::ros-info "Door in front of forum is closed.") + (speak-jp "ドアが閉まっています。") + (return-from check-forum-door-open nil) + )) + + +(defun push-forum-door-button (&optional (timeout 15000)) + (let (bboxes-msg bbox cam->button-coords cam-coords forum-door-button-pos + target-coords ray-vec via-coords push-coords + av0 av1 (arm :rarm)) + ;; Find door button + (ros::ros-info "Looking for door button...") + (speak-jp "ボタンを探しています。") + (update-robot-position) + (setq bboxes-msg + (one-shot-subscribe + "door_button/cluster_indices_decomposer/boxes" + jsk_recognition_msgs::BoundingBoxArray + :timeout timeout ;; timeout[msec] + :after-stamp (ros::time+ (ros::time-now) (ros::time 0.5)))) + (when (or (null bboxes-msg) (null (send bboxes-msg :boxes))) + (ros::ros-warn + (format nil "Button bounding box was not found in ~A[msec]." timeout)) + (return-from push-forum-door-button nil)) + (setq bbox (elt (send bboxes-msg :boxes) 0)) ;; first (expected biggest) + (setq cam->button-coords (ros::tf-pose->coords (send bbox :pose))) + (setq cam-coords (send (send *pr2* :head_mount_kinect_rgb_optical_frame_lk) + :copy-worldcoords)) + (setq forum-door-button-pos + (send (send (send cam-coords :copy-worldcoords) + :transform cam->button-coords) + :worldpos)) + (setq target-coords (make-coords :pos forum-door-button-pos)) + + ;; Define via-coords (70mm in front of button) and push-coords (push 10mm) + (setq ray-vec + (normalize-vector + (v- (float-vector + (elt (send *pr2* :laser_tilt_mount_link_lk :worldpos) 0) + (elt (send *pr2* :laser_tilt_mount_link_lk :worldpos) 1) + (elt (send target-coords :worldpos) 2)) + (send target-coords :worldpos)))) + (setq via-coords (send (send target-coords :copy-worldcoords) + :translate (scale 70 ray-vec) :world)) + (setq push-coords (send (send target-coords :copy-worldcoords) + :translate (scale -10 ray-vec) :world)) + + (ros::ros-info "Pushing button...") + (speak-jp "ボタンを押します。") + + ;; We have to reset arm pose before pushing button. + (send *ri* :start-grasp arm) + (send *pr2* :reset-pose) + (send *pr2* :head :look-at + (send (send *scene* :spot "/eng2/2f/forum-door-button") :worldpos)) + (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) + (send *ri* :wait-interpolation) + + ;; Push door button + (unless (and (setq av0 (send *pr2* arm :inverse-kinematics via-coords + :rotation-axis :x + :look-at-target t)) + (setq av1 (send *pr2* arm :inverse-kinematics push-coords + :rotation-axis :x + :look-at-target t))) + (return-from push-forum-door-button nil)) + (send *ri* :angle-vector-sequence (list av0 av1 av0) (list 3000 2000 1000)) + (send *ri* :wait-interpolation) + (pr2-tuckarm-pose) + (update-robot-position) + )) + + +(defun open-forum-door () + (call-service "door_button/pass_through_image/request" + (instance std_srvs::EmptyRequest :init)) + (until (check-forum-door-open) + (push-forum-door-button) + (unix:sleep 1)) ;; Wait for door opening + (call-service "door_button/pass_through_image/stop" + (instance std_srvs::EmptyRequest :init)) + (look-front)) + + +(defun go-to-subway-front () + (clear-costmap) + ;; TODO(Unknown): Wait in line. + (send *ri* :move-to + (send *scene* :spot "/eng2/2f/subway-front") + :retry 100)) + + +(defun order-sandwich + (sandwich + &key (bread "オススメ") (topping "オススメ") + (vegetable "オススメ") (dressing "オススメ")) + (ros::ros-info "Looking for clerk...") + (send *pr2* :head :angle-vector #f(-25 0)) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500 :head-controller) + (call-service "look_at_human/start" (instance std_srvs::EmptyRequest :init)) + (unix:sleep 5) + (ros::ros-info "Ordering sandwich...") + (ros::ros-info (format nil "Sandwich: ~A" sandwich)) + (ros::ros-info (format nil "Bread: ~A" bread)) + (ros::ros-info (format nil "Topping: ~A" topping)) + (ros::ros-info (format nil "Vegetable: ~A" vegetable)) + (ros::ros-info (format nil "Dressing: ~A" dressing)) + (ros::ros-info "Side Menu: nil") + (ros::ros-info "Others: leave it to the clerk") + (ros::ros-info "To go (Take away)") + (speak-jp "こんにちは。おつかいに来ました。" :wait t) + (speak-jp (format nil "~Aをください。" sandwich) :wait t) + (speak-jp (format nil "パンの種類は、~Aでお願いします。" bread) :wait t) + (speak-jp (format nil "トッピングは、~Aでお願いします。" topping) :wait t) + (speak-jp (format nil "野菜は、~Aでお願いします。" vegetable) :wait t) + (speak-jp (format nil "ドレッシングは、~Aでお願いします。" dressing) :wait t) + (speak-jp "サイドメニューは、要りません。" :wait t) + (speak-jp "あとは、おまかせでお願いします。" :wait t) + (speak-jp "サンドイッチは、持ち帰ります。" :wait t) ;; Reduce tax (2019/10/1~) + (call-service "look_at_human/stop" (instance std_srvs::EmptyRequest :init))) + + +(defun receive-sandwich () + ;; Go to register front + (clear-costmap) + (send *ri* :move-to + (send (send (send *scene* :spot "/eng2/2f/subway-register") + :copy-worldcoords) :translate #f(0 -200 0) :world) + :retry 100) + + ;; TODO(Unknown): Pay money here. + ;; Consider using SUB CLUB card (pre-paid card). + ;; Don't forget to get receipt and put it into the bag. + + (send *pr2* :angle-vector (pr2-pick-bag-pose)) + (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) + (send *ri* :wait-interpolation) + + ;; Go to register + (speak-jp "少し前に出ます。" :wait t) + (send *ri* :go-pos-unsafe 0.2 0 0) + + (send *ri* :stop-grasp :rarm :wait t) + + (ros::ros-info "Waiting for receiving sandwich.") + (speak-jp "商品をください。" :wait t) + (speak-jp "袋を私の手に掛けて、手を揺らしてください。" :wait t) + + (wait-for-hand-over :rarm 60 10) ;; wait at most 10 min + + (send *ri* :start-grasp :rarm) + + (speak-jp "後ろに下がります。気をつけてください。" :wait t) + (clear-costmap) + (send *ri* :go-pos-unsafe -0.5 0 0) + + (send *pr2* :torso_lift_joint :joint-angle 0) + (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) + (send *ri* :wait-interpolation) + + ;; Prevent sandwich from covering /base_scan region. + (pr2-tuckarm-pose :rarm) + (look-front) + + (send *ri* :go-pos-unsafe 0 0 180) + t) + + +(defun go-to-initial-pose () + (ros::ros-info "Coming back to original position...") + (speak-jp "もとの場所に帰ります。" :wait t) + + ;; NOTE: DWA local planner does not publish command to go through the door + ;; when detecting door closed, so move to forum-door-outside first + ;; in order to approach the door. + (ros::ros-info "Move to /eng2/2f/forum-door-outside.") + (send *ri* :move-to (send *scene* :spot "/eng2/2f/forum-door-outside")) + + ;; NOTE: Robot does not have student ID card, so please open door when needed. + (ros::ros-info "Move to elevator hall.") + (send *ri* :move-to + (send (send (send *scene* :spot "/eng2/2f/forum-door-inside") + :copy-worldcoords) :translate #f(-1500 -2000 0) :world)) + + (ros::ros-info (format nil "Move to initial pose: ~A." *initial-pose*)) + (call-service "look_at_human/start" (instance std_srvs::EmptyRequest :init)) + (send *ri* :move-to *initial-pose*)) + + +(defun pass-sandwich (sandwich) + (call-service "look_at_human/stop" (instance std_srvs::EmptyRequest :init)) + (update-robot-position) + (send *ri* :go-pos-unsafe 0 0 (send *pr2* :head :neck-y :joint-angle)) + (look-front) + + (send *pr2* :angle-vector (pr2-pick-bag-pose)) + (send *pr2* :head :angle-vector #f(0 0)) + (send *ri* :angle-vector (send *pr2* :angle-vector) 5000) + (send *ri* :wait-interpolation) + (send *ri* :stop-grasp :rarm :wait t) + (speak-jp (format nil "~Aを買ってきました。" sandwich) :wait t) + + (until (wait-for-hand-impact :rarm :timeout 30) + (ros::ros-info "Still waiting for a human to get sandwich from PR2.") + (speak-jp (format nil "~Aをどうぞ。" sandwich) :wait t)) + + (send *ri* :start-grasp :rarm) + (send *pr2* :torso_lift_joint :joint-angle 0) + (look-front) + (pr2-tuckarm-pose)) + + +(defun price (sandwich) + (cond + ((find sandwich + (list "BLTサンド" "ローストチキンサンド" "ツナサンド") + :test #'string=) + 421) ;; 390 * 1.08 = 421 yen (2019/12/27) + (t nil))) + + +(defun demo (&key (sandwich "BLTサンド")) + (init) + (speak-jp (format nil "~Aを買いに行きます。" sandwich) :wait t) + (speak-jp + (format nil "おそらく、税込み~A円になります。" (price sandwich)) :wait t) + (go-to-forum-door-inside) + (open-forum-door) + (go-to-subway-front) + (order-sandwich sandwich) + (receive-sandwich) + (go-to-initial-pose) + (pass-sandwich sandwich)) + + +(defun main () + (ros::roseus "fetch_sandwich") + (let ((sandwich-en (or (string-downcase (get-arg :object)) "blt_sand")) + (sandwich-jp nil) + (sandwiches '(("blt_sand" "BLTサンド") + ("roast_chicken_sand" "ローストチキンサンド") + ("tuna_sand" "ツナサンド")))) + (dolist (s sandwiches) + (when (string= sandwich-en (elt s 0)) + (setq sandwich-jp (elt s 1)))) + (when (null sandwich-jp) + (speak-jp "すみません、聞き取れませんでした。") + (exit 1)) + (demo :sandwich sandwich-jp) + (exit 0))) diff --git a/elevator_move_base_pr2/sample/fetch_sandwich.launch b/elevator_move_base_pr2/sample/fetch_sandwich.launch new file mode 100644 index 0000000000..ec9b8404cd --- /dev/null +++ b/elevator_move_base_pr2/sample/fetch_sandwich.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + nodes: + - /elevator_move_base + - /fetch_sandwich + - /human_in_mirror/find_human_in_mirror + - /narrow_stereo/left/find_elevator_button + exclude_regexes: + - ".*follow_joint_trajectory.*" + - ".*goal id.*" + - ".*violate.*" + - ".*wait-interpolation debug.*" + - ".*action-result-cb.*" + - ".*;; .*" + - ".*#f(.*).*" + - ".*commanded joint differs.*" + - ".*trajectory command :.*" + - ".*waitForTransform failed! :.*" + reverse_lines: false + line_buffer_length: 15 + + + + diff --git a/elevator_move_base_pr2/sample/go-to-spot.l b/elevator_move_base_pr2/sample/go-to-spot.l new file mode 100755 index 0000000000..778aec3d4e --- /dev/null +++ b/elevator_move_base_pr2/sample/go-to-spot.l @@ -0,0 +1,40 @@ +#!/usr/bin/env roseus +;; go-to-room.l +;; Author: furushchev + +(require :pr2-interface "package://pr2eus/pr2-interface.l") +(require :eng2-scene "package://jsk_maps/src/eng2-scene.l") + + +(defvar *pr2*) +(defvar *ri*) +(defvar *scene*) +(defvar *spot*) +(defvar *spot-name*) + + +(defun pr2-init (&optional (create-viewer)) + (setq *pr2* (pr2) + *ri* (instance pr2-interface :init + :move-base-action-name "elevator_move_base")) + (ros::spin-once) + (send *ri* :spin-once) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + (when create-viewer (objects (list *pr2*)))) + +(defun main () + (ros::roseus "pr2_go_to_room") + + (pr2-init) + (setq *scene* (make-eng2-scene)) + + (setq *spot-name* (ros::get-param "~destination" "")) + (setq *spot* (send *scene* :spot *spot-name*)) + (unless *spot* + (ros::ros-error "Destination '~A' not found" *spot-name*) + (return-from main)) + + (send *ri* :move-to *spot*) + (return-from main t)) + +(main) diff --git a/elevator_move_base_pr2/scripts/button_state_marker.l b/elevator_move_base_pr2/scripts/button_state_marker.l deleted file mode 100644 index 027978ff5e..0000000000 --- a/elevator_move_base_pr2/scripts/button_state_marker.l +++ /dev/null @@ -1,62 +0,0 @@ -#!/usr/bin/env roseus - -(ros::roseus "button_state_marker") -(ros::roseus-add-msgs "visualization_msgs") - -(defun text-marker (str &key (pose (make-coords)) (frame_id "base_link") - (size 0.2) (color (float-vector 1 0 0)) (alpha 1.0)) - (let ((msg (instance visualization_msgs::marker :init))) - (send msg :header :frame_id frame_id) - (send msg :header :stamp (ros::time-now)) - (send msg :type visualization_msgs::tet_view_facing) - (send msg :pose (ros::coords->tf-pose pose)) - (send msg :scale :z size) - (send msg :color :r (elt color 0)) - (send msg :color :g (elt color 1)) - (send msg :color :b (elt color 2)) - (send msg :color :a alpha) - (send msg :frame_locked t) - (send msg :text str) - msg)) - - - -(ros::roseus-add-msgs "image_view2") -(defun text-image-marker (str) - (let ((msg (instance image_view2::ImageMarker2 :init))) - (send msg :type image_view2::TEXT) - (send msg :text str) - msg )) - - -(setq *button-chk* '(nil nil)) -(defun button-point-cb (msg) ;; Point - (let (mrk (instance image_view2::ImageMarker2 :init)) - (send mrk :type image_view2::CIRCLE) - (send mrk :position (send msg :pose :position)) - (send mrk :scale (send pos :z)) ;; this is proocol - (send mrk :lifetime (ros::time 1.0)) - (ros::publish "/image_marker" mrk) - )) -(defun button-light-cb (msg) ;; Float32 - (let ((mrk (text-image-marker (string (send msg :data))))) - (send mrk :position :x ) - (send mrk :lifetime (ros::time 1.0)) - (ros::publish "/image_marker" mrk) - )) - -(defun elevator-number-cb (msg) ;; string - (let ((mrk (text-image-marker (string (send msg :data))))) - (send mrk :position :x 400) - (send mrk :position :y 200) - (send mrk :lifetime (ros::time 1.0)) - (ros::publish "/image_marker" mrk) - )) - -(defun object-detection-cb (msg) ;; ObjectDetection - (let ((mrk (instance image_view2::ImageMarker2 :init))) - (send mrk :lifetime (ros::time 1.0)) - (ros::publish "/image_marker" mrk) - )) - -(ros::advertise "/image_marker" image_view2::ImageMarker2) diff --git a/elevator_move_base_pr2/scripts/floor_number_marker.py b/elevator_move_base_pr2/scripts/floor_number_marker.py deleted file mode 100755 index e69de29bb2..0000000000 diff --git a/elevator_move_base_pr2/scripts/install_trained_model.py b/elevator_move_base_pr2/scripts/install_trained_model.py new file mode 100755 index 0000000000..862e6fdbc7 --- /dev/null +++ b/elevator_move_base_pr2/scripts/install_trained_model.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python + +import argparse +import multiprocessing + +import jsk_data + + +def download_data(*args, **kwargs): + p = multiprocessing.Process( + target=jsk_data.download_data, + args=args, + kwargs=kwargs) + p.start() + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument('-v', '--verbose', dest='quiet', action='store_false') + args = parser.parse_args() + quiet = args.quiet + + PKG = 'elevator_move_base_pr2' + + download_data( + pkg_name=PKG, + path='trained_model/fcn8s_at_once_door_button_model_20191025.npz', + url='https://drive.google.com/uc?id=1CmEWO0kyTPHrQ-D4kb8bIy3yGsWV3RBd', + md5='f255461e5b04a0b05e04b5aedefa5618', + quiet=quiet, + ) + + +if __name__ == '__main__': + main() diff --git a/elevator_move_base_pr2/src/add-msg-pgsql.l b/elevator_move_base_pr2/src/add-msg-pgsql.l deleted file mode 100644 index 1d707f30b5..0000000000 --- a/elevator_move_base_pr2/src/add-msg-pgsql.l +++ /dev/null @@ -1,157 +0,0 @@ -;; postgresql Euslisp interface -(require :pqsql (ros::resolve-ros-path "package://euslisp/jskeus/eus/lib/llib/pgsql.l")) - -(defun init-pgsql-safe (&key (host "localhost") (port 0) (options 0) (tty 0) - (dbname (cond ((unix:getenv "PGDATABASE")) - (t (unix:getenv "USER")))) - (user (cond ((unix:getenv "PGUSER")) - (t (unix:getenv "USER")))) - (pass 0)) - (let ((connection (pq::setdblogin host port options tty dbname user pass))) - (when (zerop (pqstatus connection)) - (instance pgsql :init :host host :port port :options options - :tty tty :dbname dbname :user user :pass pass) - ))) - -;; initialize db connection -(unless (boundp '*db*) - (setq *db* (init-pgsql-safe :dbname "pr2db" :user "pr2admin" :host "pr1012")) - (if (null *db*) (warn "pgsql can't open."))) - -;; overwrite function -(defun tables (db) - (pq:query db nil "select tablename from pg_tables where schemaname='public';")) - -;; insert-trial-table (db text rostime) -;; insert-tf-table (db msg) -;; insert-tf-table-by-coords (db stamp parent child coords) - -(defun create-trial-table (db) - (pq:query db nil "CREATE TABLE trial (id serial,type text,stamp bigint);")) -(defun insert-trial-table (db text rostime) - (when db - (pq:insert-record2 db "trial" '(type stamp) - (list text (format nil "~d~0,9d" (send rostime :sec) - (send rostime :nsec)))))) - -(defun _db-values-by-accessors (msg accessors) - (mapcar #'(lambda(acc) - (let ((m msg)) - (dolist (a acc m) - (setq m (if (keywordp a) (send m a) (elt m a))) - (when (eq (class m) time) - (setq m (send m :to-nsec)))))) - accessors)) - -(defun _create-msg-table (db table fields types) - (let (definition str) - (while (or fields types) - (push "," definition) - (push (car fields) definition) - (push " " definition) - (push (car types) definition) - (setq fields (cdr fields) types (cdr types))) - (setq str (apply #'concatenate string (mapcar #'string-downcase (reverse definition)))) - (pq:query db nil (format nil "CREATE TABLE ~a (id serial ~a);" table str)) - )) - -(defun _insert-msg-table (db table msg fields accessors) - (let ((values (_db-values-by-accessors msg accessors))) - (when db - (pq:insert-record2 db table fields values)) - )) - -;; -(defun create-tf-table (db &key (table "tf")) - (let ((fields - '(header_stamp header_frame_id child_frame_id - transform_translation_x transform_translation_y - transform_translation_z transform_rotation_x - transform_rotation_y transform_rotation_z transform_rotation_w)) - (types '(bigint text text real real real real real real real))) - ;; create table - (_create-msg-table db table fields types) - ;; add index on timestamp - (pq:query db nil (format nil "CREATE INDEX ~A_stamp_idx ON ~A(header_stamp);" table table)) - )) - -(defun insert-tf-table (db msg &key (table "tf")) - (let ((fields - '(header_stamp header_frame_id child_frame_id - transform_translation_x transform_translation_y - transform_translation_z transform_rotation_x - transform_rotation_y transform_rotation_z transform_rotation_w)) - (accessors - '((:header :stamp) (:header :frame_id) (:child_frame_id) - (:transform :translation :x) (:transform :translation :y) - (:transform :translation :z) - (:transform :rotation :x) (:transform :rotation :y) - (:transform :rotation :z) (:transform :rotation :w)))) - (_insert-msg-table db table msg fields accessors) - )) - - -(defun insert-tf-table-by-coords (db stamp source-obj target-obj - &key source_id target_id (table "tf")) - (let* ((coords (send (make-coords);;(send target-obj :copy-worldcoords) - :transform (send source-obj :transformation target-obj) :world)) - (parent (or source_id (send source-obj :name))) - (child (or target_id (send target-obj :name))) - (msg (ros::coords->tf-transform-stamped coords parent child))) - (send msg :header :stamp stamp) - (insert-tf-table db msg :table table) - )) - -(defun select-tf-from-table (db &key frame_id child_id after before (limit 1) (table "tf") (with-time nil)) - (when (derivedp after time) (setq after (send after :to-nsec))) - (when (derivedp before time) (setq before (send before :to-nsec))) - (let ((fields - '(header_stamp header_frame_id child_frame_id - transform_translation_x transform_translation_y - transform_translation_z transform_rotation_x - transform_rotation_y transform_rotation_z transform_rotation_w)) - (cond1 (if frame_id (list '= 'header_frame_id frame_id))) - (cond2 (if child_id (list '= 'child_frame_id child_id))) - (cond3 (if after (list '> 'header_stamp after))) - (cond4 (if before (list '< 'header_stamp before))) - conslist where-cond result qu co - return-list) - (setq condlist (delete nil (list cond1 cond2 cond3 cond4))) - (setq where-cond (cond ((<= 2 (length condlist)) - (append '(and) condlist)) - ((car condlist) (car condlist)) - (t ""))) - (setq result (select db fields table :where where-cond :order-by "header_stamp DESC" :limit limit)) - (when result - (dolist (record result) - (setq qu (subseq record 6)) - (setq co (make-coords :pos (scale 1000 (coerce (subseq record 3 6) float-vector)) - :rot (quaternion2matrix (float-vector (elt qu 3) (elt qu 0) (elt qu 1) (elt qu 2))))) - (if with-time - (push (cons co (send (ros::time) :from-nsec (elt record 0))) return-list) - (push co return-list))) - return-list) - )) - -;; ros message types -> bool int8 uint8 int16 uint16 int32 uint32 int64 uint64 -;; float32 float64 string time duration and [] - - -;; publish posearray for "success" "fail" -(defun publish-posearray (topic-name type) - (let ((frame_id "/eng8") tmlist tflist - (msg (instance geometry_msgs::PoseArray :init))) - - (ros::advertise topic-name geometry_msgs::PoseArray 1) - (unix::sleep 1) - - (setq tmlist (flatten (pq:query *db* nil (format nil "SELECT stamp FROM trial WHERE type LIKE '~A%';" type)))) - (setq tflist (mapcan #'(lambda(tm)(select-tf-from-table *db* :before tm :frame_id frame_id)) tmlist)) - - (send msg :header :stamp (ros::time-now)) - (send msg :header :frame_id frame_id) - (send msg :poses (mapcar #'ros::coords->tf-pose tflist)) - - (ros::publish topic-name msg) -)) - diff --git a/elevator_move_base_pr2/src/color_point_detector.cpp b/elevator_move_base_pr2/src/color_point_detector.cpp index 88f55b1eef..bd8cf47778 100644 --- a/elevator_move_base_pr2/src/color_point_detector.cpp +++ b/elevator_move_base_pr2/src/color_point_detector.cpp @@ -1,3 +1,7 @@ +#include +#include +#include + #include #include #if ROS_VERSION_MINIMUM(1, 15, 0) @@ -12,7 +16,6 @@ #include #include #include -#include class FrameDrawer { @@ -26,12 +29,13 @@ class FrameDrawer image_geometry::PinholeCameraModel cam_model_; std::string current_frame_; - double x,y,r; + double x_, y_, r_; #if ROS_VERSION_MINIMUM(1, 15, 0) cv::Scalar target_color_; #else CvScalar target_color_; #endif + std::vector decay_bgr_; std::vector button_pose_; @@ -54,14 +58,22 @@ class FrameDrawer #else target_color_ = CV_RGB(r,g,b); #endif + + double decay_r, decay_g, decay_b; + private_nh_.param("decay_r", decay_r, 0.1); + private_nh_.param("decay_g", decay_g, 0.1); + private_nh_.param("decay_b", decay_b, 0.1); + decay_bgr_.push_back(decay_b); + decay_bgr_.push_back(decay_g); + decay_bgr_.push_back(decay_r); } void pointCb(const geometry_msgs::PointStamped::ConstPtr& point_msg) { current_frame_ = point_msg->header.frame_id; - x = point_msg->point.x; - y = point_msg->point.y; - r = point_msg->point.z; // radius on image coordinate + x_ = point_msg->point.x; + y_ = point_msg->point.y; + r_ = point_msg->point.z; // radius on image coordinate } void imageCb(const sensor_msgs::ImageConstPtr& image_msg, @@ -72,53 +84,58 @@ class FrameDrawer //std::cout << "."; // drop 9 of 10 frames - //if(info_msg->header.seq % 10 != 0) return; - if(image_msg->header.frame_id != current_frame_) return; + //if(info_msg->header.seq % 10 != 0) {return;} + if(image_msg->header.frame_id != current_frame_) {return;} - try { + try + { // May want to view raw bayer data // NB: This is hacky, but should be OK since we have only one image CB. if (image_msg->encoding.find("bayer") != std::string::npos) - boost::const_pointer_cast(image_msg)->encoding = "mono8"; + { + boost::const_pointer_cast(image_msg)->encoding = "mono8"; + } //image = bridge_.imgMsgToCv(image_msg, "bgr8"); cv_ptr = cv_bridge::toCvCopy(image_msg, "bgr8"); image = cv_ptr->image; } - catch (cv_bridge::Exception& ex) { + catch (cv_bridge::Exception& ex) + { ROS_ERROR("[draw_frames] Failed to convert image"); return; } // output debug image - //sensor_msgs::CvBridge debug_bridge; - //debug_bridge.fromImage (*image_msg, "bgr8"); - //IplImage* src_imgipl = debug_bridge.toIpl(); - //cv::Mat img(src_imgipl); - cv::Mat img = image; - + cv::Mat debug_img = image.clone(); + // calcurate the score double max_score = -1e9; int ix[] = {0,1,0,-1,0}, iy[] = {0,0,1,0,-1}; - for(int ind=0; ind<5; ind++) { + for(int ind=0; ind<5; ind++) + { std::vector colbuf; - for(int i = 0; i < 121; i++){ - double px = i/11, py = i%11; - cv::Point2d uv(x + (5-px)*r/5 + r*ix[ind], y + (5-py)*r/5 + r*iy[ind]); - if(0<=uv.x && uv.x < image.size().width-1 && 0<=uv.y && uv.y < image.size().height-1){ - colbuf.push_back(image.at((int)uv.y, (int)uv.x)); - } + for(int i = 0; i < 121; i++) + { + double px = i/11, py = i%11; + cv::Point2d uv(x_ + (5-px)*r_/5 + r_*ix[ind], y_ + (5-py)*r_/5 + r_*iy[ind]); + if(0<=uv.x && uv.x < image.size().width && 0<=uv.y && uv.y < image.size().height) + { + colbuf.push_back(image.at(static_cast(uv.y), static_cast(uv.x))); + } } // check yellow point std::vector vscore; for(std::vector::iterator it=colbuf.begin();it!=colbuf.end();it++) - { - double lscore = 0.0; - for(int i=0;i<3;i++) - lscore += exp(-abs(target_color_.val[i] - (*it)[i])/10.0); - vscore.push_back(lscore); - } + { + double lscore = 0.0; + for(int i=0;i<3;i++) + { + lscore += exp(-abs(target_color_.val[i] - (*it)[i]) * decay_bgr_[i]); + } + vscore.push_back(lscore); + } std::sort(vscore.begin(), vscore.end()); double score = vscore[vscore.size()*3/4]; @@ -126,32 +143,35 @@ class FrameDrawer } // for debug image + cv::circle(debug_img, cv::Point(static_cast(x_), static_cast(y_)), #if ROS_VERSION_MINIMUM(1, 15, 0) - cv::circle(img, cv::Point2f(x, y), r, cv::Scalar(0,0,255), 3); + static_cast(r_), cv::Scalar(0,0,255), #else - cv::circle(img, cv::Point2f(x, y), r, CV_RGB(255,0,0), 3); + static_cast(r_), CV_RGB(255,0,0), #endif + 3); char text[32]; - sprintf(text, "brightness = %.3f", max_score); - cv::putText (img, std::string(text), cv::Point(x-100, y+70+r), + snprintf(text, sizeof(text), "brightness = %.3f", max_score); + cv::putText(debug_img, std::string(text), + cv::Point(static_cast(x_ - 100), static_cast(y_ + 70 + r_)), + #if ROS_VERSION_MINIMUM(1, 15, 0) - 0, 0.7, cv::Scalar(0,0,255), + 0, 0.7, cv::Scalar(0,0,255), #else - 0, 0.7, CV_RGB(255,0,0), + 0, 0.7, CV_RGB(255,0,0), #endif - 2, 8, false); + 2, 8, false); std_msgs::Float32 score_msg; - score_msg.data = max_score; + score_msg.data = static_cast(max_score); result_pub_.publish(score_msg); // publish debug image - //cv_bridge::CvImage out_msg; - //out_msg.header = image_msg->header; - //out_msg.encoding = "bgr8"; - //out_msg.image = img; - //debug_pub_.publish(out_msg.toImageMsg()); - debug_pub_.publish(cv_ptr->toImageMsg()); + cv_bridge::CvImage out_msg; + out_msg.header = image_msg->header; + out_msg.encoding = "bgr8"; + out_msg.image = debug_img; + debug_pub_.publish(out_msg.toImageMsg()); } }; diff --git a/elevator_move_base_pr2/src/database-interface.l b/elevator_move_base_pr2/src/database-interface.l index c2f62ef9d3..b30b890442 100644 --- a/elevator_move_base_pr2/src/database-interface.l +++ b/elevator_move_base_pr2/src/database-interface.l @@ -1,54 +1,52 @@ (require :mongo-client "package://roseus_mongo/euslisp/mongo-client.l") -(when (and (ros::has-param "robot/name") - (string= "pr1012" (ros::get-param "robot/name"))) - (setq json::*tzoffset* -8) - (ros::ros-warn "time zone offset is changed to -8 (PST)")) -;;;;;; insert +;; (defvar *pr2*) +;; (defvar *ri*) + (defclass mongo-logger :slots (task-id) :super propertied-object) + (defmethod mongo-logger (:init (&optional (name "")) (setq task-id (format nil "~A~A" name (gensym))) (ros::ros-warn "logger is initialized task-id: ~A" task-id) t) + (:save-robot-pose (&optional comment) - (when *logging* - (let ((c (send *ri* :state :worldcoords)) docid) - (ros::ros-warn "saving robot pose...") - (setq docid - (mongo::insert - (ros::coords->tf-pose-stamped c (send c :name)) - :meta `((:comment . ,comment) - (:task-id . ,task-id)))) - (ros::ros-warn "saved robot pose ~A" docid))) + (let ((c (send *ri* :state :worldcoords)) docid) + (ros::ros-warn "saving robot pose...") + (setq docid + (mongo::insert + (ros::coords->tf-pose-stamped c (send c :name)) + :meta `((:comment . ,comment) + (:task-id . ,task-id)))) + (ros::ros-warn "saved robot pose ~A" docid)) t) + (:save-obj-pose (obj &optional comment) - (when *logging* - (let ((robot->obj (send *pr2* :transformation obj)) - docid) - (ros::ros-warn "saving obj pose ~A..." (send obj :name)) - (setq docid - (mongo::insert - (ros::coords->tf-pose-stamped robot->obj "/base_footprint") - :meta `((:object . ,(send obj :name)) - (:task-id . ,task-id) - (:comment . ,comment)))) - (ros::ros-warn "saved obj pose ~A: ~A" (send obj :name) docid))) - t) - (:save-brightness (value &optional comment) - (when *logging* - (ros::ros-warn "saving brightness ~A" value) + (let ((robot->obj (send *pr2* :transformation obj)) docid) + (ros::ros-warn "saving obj pose ~A..." (send obj :name)) (setq docid (mongo::insert - (instance std_msgs::float32 :init :data value) - :meta `((:comment . ,comment) - (:task-id . ,task-id))))) + (ros::coords->tf-pose-stamped robot->obj "/base_footprint") + :meta `((:object . ,(send obj :name)) + (:task-id . ,task-id) + (:comment . ,comment)))) + (ros::ros-warn "saved obj pose ~A: ~A" (send obj :name) docid)) + t) + + (:save-brightness (value &optional comment) + (ros::ros-warn "saving brightness ~A" value) + (setq docid + (mongo::insert + (instance std_msgs::float32 :init :data value) + :meta `((:comment . ,comment) + (:task-id . ,task-id)))) t) ) -(provide :database-interface) +(provide :database-interface) diff --git a/elevator_move_base_pr2/src/elevator-buttons.l b/elevator_move_base_pr2/src/elevator-buttons.l deleted file mode 100644 index 83d5d3df94..0000000000 --- a/elevator_move_base_pr2/src/elevator-buttons.l +++ /dev/null @@ -1,20 +0,0 @@ -(load "package://jsk_maps/src/eng2-scene.l") -(load "package://jsk_maps/src/eng8-scene.l") - -;; -(defun update-scene-by-tf (scene &optional (tm (ros::time 0))) - (let* ((objects (send scene :objects)) - (fixed-frame (send scene :name)) ;; ok? - (updated nil)) - (dolist (obj objects) - (when (and (stringp (send obj :name)) - (send *tfl* :can-transform fixed-frame (send obj :name) tm)) - (let ((trans (send *tfl* :lookup-transform - fixed-frame (send obj :name) tm))) - (send obj :move-to trans scene) - (push obj updated) - ;;(ros::ros-info "trans ~A is ~A" obj trans) - ))) - updated - )) - diff --git a/elevator_move_base_pr2/src/elevator-move-base-main.l b/elevator_move_base_pr2/src/elevator-move-base-main.l deleted file mode 100644 index ba5081f934..0000000000 --- a/elevator_move_base_pr2/src/elevator-move-base-main.l +++ /dev/null @@ -1,437 +0,0 @@ -#!/usr/bin/env roseus - -(ros::load-ros-manifest "elevator_move_base_pr2") - -(defvar *logging* (ros::get-param "logging" nil)) - -(if *logging* - (progn - (require :attention-observation "package://jsk_demo_common/euslisp/attention-observation.l") - (defparameter *mongo-database* "jsk_robot_lifelog") - (defparameter *current-context* :elevator)) - (progn - (require :pr2-interface "package://pr2eus/pr2-interface.l") - (unless (boundp 'make-attention-action) - (setf (symbol-function 'make-attention-action) (symbol-function 'defun))))) - -;(load "package://elevator_move_base_pr2/src/add-msg-pgsql.l") -(require :database-interface "database-interface.l") -(load "package://elevator_move_base_pr2/src/navigation-client.l") -(load "package://elevator_move_base_pr2/src/push-elevator-button.l") -(load "package://elevator_move_base_pr2/src/ros-callback-manager.l") - - -(load "package://roseus_smach/src/state-machine-actionlib.l") - -(load "package://elevator_move_base_pr2/src/move-inside-elevator.l") - -(setq *node-debug* nil) -;(ros::service-call "~set_logger_level" -; (instance roscpp::SetLoggerLevelRequest -; :init :logger "ros" :level "DEBUG")) - -;; これどうしよう -(setq *floors* '(("B2F" "ちかにかい") ("B1F" "ちかいっかい") - ("1F" "いっかい") ("2F" "にかい") - ("3F" "さんかい") ("4F" "よんかい") - ("5F" "ごかい") ("6F" "ろくかい") - ("7F" "ななかい") ("8F" "はちかい") - ("9F" "きゅうかい") ("10F" "じっかい") - ("11F" "じゅういっかい") ("12F" "じゅうにかい"))) - -(defun initialize-env () - (defvar *tfl* (instance ros::transform-listener :init)) - - (ros::advertise "robotsound" sound_play::SoundRequest 5) - (ros::advertise "view_target" geometry_msgs::PointStamped 1) - - ;; (setq *ri* (instance pr2-interface :init)) - ;; (setq *pr2* (pr2)) - (pr2-init) - - ;; load building scene by existing TF frame - (unix::sleep 1) ;; wait for recieving TF messages - (let ((frames (send *tfl* :get-frame-strings))) - (cond ((find "eng2" frames :test #'string=) - (setq *scene* (make-eng2-scene))) - ((find "eng8" frames :test #'string=) - (setq *scene* (make-eng8-scene))) - (t - (ros::ros-error "There is no building frame-id in TF tree.") - (exit)) - )) - - (when *node-debug* - (if (not (boundp '*irtviewer*)) (make-irtviewer)) - (objects (list *pr2* *scene*)) - (send *irtviewer* :look-all *pr2*) - (send *irtviewer* :draw-objects) - (x::window-main-one) - ) - ) - -(defun update-robot-position () - (ros::spin-once) - (send *pr2* :move-to (send *ri* :state :worldcoords) :world) - (send *pr2* :angle-vector (send *ri* :state)) - ) - -(defun update (&optional (times 100)) - (let (updated-object (count 0) (found 0) (panel-moved nil)) - ;; wait for result - (ros::rate 10) - - (while (<= (incf count) times) - (ros::spin-once) - - (update-robot-position) - (setq updated-object (update-scene-by-tf *scene*)) - - (when (memq *panel* updated-object) - (setq panel-moved t) - (incf found)) - ;; - (print (list 'count count 'found found - (send *panel* :worldpos))) - ;; - (when *node-debug* - (ros::spin-once) - (send *pr2* :angle-vector (send *ri* :state)) - (send *irtviewer* :look-all *pr2*) - (send *irtviewer* :draw-objects) - (x::window-main-one)) - (if (>= found 5) (return)) - (ros::sleep)) - panel-moved - )) - -(defun transform-pose-to-robot-coords (coords) - (transform-pose-to-target-frame coords "base_footprint")) - -(defun set-view-target (coords camera frame-id) - (let ((msg (instance geometry_msgs::PointStamped :init)) - (uv (send camera :screen-point (send coords :worldpos)))) - ;; out of view - (when (not (and (<= 0 (elt uv 0) (send camera :width)) - (<= 0 (elt uv 1) (send camera :height)))) - (ros::ros-error "screen-point: ~A is out of view (~A, ~A)" - uv (send camera :width) (send camera :height)) - (return-from set-view-target nil)) - (send msg :header :frame_id frame-id) - (send msg :point :x (elt uv 0)) - (send msg :point :y (elt uv 1)) - (send msg :point :z 10) ;; radius [px] - (ros::publish "view_target" msg) - t)) - -(defclass light-button-cb-class - :super propertied-object - :slots (value timestamp tau)) -(defmethod light-button-cb-class - (:init () - (setq value 0 timestamp (ros::time) tau 1.0) ;; tau = half-life - (ros::subscribe "light_button" std_msgs::float32 #'send self :cb)) - (:cb (msg) - (let* ((diff (send (ros::time- (ros::time-now) timestamp) :to-sec)) - (weight (exp (* (log 0.5) (/ diff tau))))) - (setq timestamp (ros::time-now)) - (setq value (+ (* weight value) (* (- 1 weight) (send msg :data)))) - )) - ;; if the last observation is too old, the score is 0 - (:value () (if (< tau (send (ros::time- (ros::time-now) timestamp) :to-sec)) - (setq value 0) value))) - -;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; - -;; -;; State Machine -;; - -(make-attention-action look-button (userdata) - (let ((panel-name (cdr (assoc 'panel-name userdata))) look-target) - (send *logger* :save-robot-pose "look-button") - ;; nearest call panel - (ros::ros-info "panel-name: ~A" panel-name) - (setq *panel* (car (send *scene* :find-object panel-name))) - (setq look-target (car (send *scene* :find-object (format nil "~A~A" (check-current-floor) panel-name)))) - ;; - (update-robot-position) - (update-scene-by-tf *scene*) - (when look-target - (ros::ros-info "look-at button position ~A" (send look-target :worldpos)) - (send *pr2* :head :angle-vector #f(0 0)) - (send *pr2* :head :look-at (send look-target :worldpos)) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) - (send *ri* :wait-interpolation)) - - (unix:sleep 2) - - ;; update - (ros::ros-info "wait vision update") - (unless (update 100) (return-from look-button nil)) - - (when *node-debug* - (send *irtviewer* :draw-objects)) - - (send *logger* :save-obj-pose *panel* "look-button") - ;;(insert-tf-table-by-coords *db* (ros::time-now) *scene* (send *pr2* :base_footprint) :target_id "/base_footprint") - t )) - -(make-attention-action push-button-func (userdata) - (let* ((button (cdr (assoc 'button userdata))) - (button-obj - (find-if #'(lambda(o)(string= button (send o :name))) (flatten (send *panel* :inheritance))))) - (ros::ros-info (format nil "push button (~a) -> ~a" button button-obj)) - (speak-jp "ぼたんをおします") - (send *logger* :save-obj-pose button-obj "push-button") - (push-button button-obj) - )) - -(make-attention-action check-button-light (userdata) - (let* ((button (cdr (assoc 'button userdata))) - (button-coords (find-if #'(lambda(o)(string= button (send o :name))) - (flatten (send *panel* :inheritance)))) - (button-state (instance light-button-cb-class :init))) - (update-robot-position) - (set-view-target button-coords (send *pr2* :wide_stereo-left) "wide_stereo_optical_frame") - ;; check phase - (dotimes (i 30 (< i 30)) ;; 3 sec - (ros::spin-once) - (send *logger* :save-brightness (send button-state :value) "check-button") - (when (< 0.5 (send button-state :value)) - (pprint 'button-light) (speak-jp "ぼたんがひかりました") -;; (pr2-tuckarm-pose) - (return)) - (unix::usleep (* 100 1000))) - )) - -(defun push-state-machine () - (let ((sm (instance state-machine :init))) - (send sm :add-node (instance state :init :look 'look-button)) - (send sm :add-node (instance state :init :push 'push-button-func)) - (send sm :add-node (instance state :init :check 'check-button-light)) - (send sm :arg-keys 'button 'panel-name) - (send sm :goal-state (list :success :fail)) - (send sm :start-state :look) - (send sm :add-transition :look :push t) - (send sm :add-transition :look :fail nil) - (send sm :add-transition :push :check t) - (send sm :add-transition :push :fail nil) - (send sm :add-transition :check :success t) - (send sm :add-transition :check :look nil) - sm )) - -(defun coords->movebaseactiongoal (co) - (let ((goal (instance move_base_msgs::movebaseactiongoal :init))) - (send goal :goal :target_pose :pose (ros::coords->tf-pose co)) - (send goal :goal :target_pose :header :frame_id (send co :name)) - goal - )) - -(defun check-until-elevator-empty (&rest args) - (let ((start (ros::time-now)) (result nil)) - (setq *scan* nil) - (ros::subscribe "/base_scan" sensor_msgs::LaserScan - #'(lambda(m)(setq *scan* m))) - (unix::sleep 1) ;; to smart - - (while (< (send (ros::time- (ros::time-now) start) :to-sec) 30) - (while (not *scan*) (ros::spin-once)) - ;; - (send *tfl* :wait-for-transform "map" (send *scan* :header :frame_id) - (send *scan* :header :stamp) 0.5) - (setq laser-pose - (send *tfl* :lookup-transform "map" (send *scan* :header :frame_id) - (ros::time))) - (let ((points nil) (angle (send *scan* :angle_min)) box) - (setq target-areas - (list (send (make-cube 1000 2000 1000) :translate #f(4200.0 -30700.0 0.0)) - (send (make-cube 1000 2000 1000) :translate #f(-25800.0 -30700.0 0.0)) - (send (make-cube 1000 2000 1000) :translate #f(34200.0 -30700.0 0.0)))) - (send-all target-areas :worldpos) ;; omajinai - (setq points - (map cons - #'(lambda(r) - (prog1 - (float-vector (* r (cos angle)) - (* r (sin angle)) 0) - (incf angle (send *scan* :angle_increment)))) - (scale 1000 (send *scan* :ranges)))) - (setq points - (mapcan #'(lambda(x) - (let ((pt (send laser-pose :transform-vector x))) - (when (memq :inside (send-all target-areas :insidep pt)) - (list pt)))) - points)) - (when (< (length points) 10) - (setq result t) (return)) - )) - (ros::unsubscribe "/base_scan") - (speak-jp (if result "エレベータに入ります" "エレベータに入れません")) - result - )) - -(defun costmap-for-elevator (userdata) - (change-inflation-range 0.2) - (switch-global-planner-observation nil) ;;disable tilt scan - t) -(defun costmap-normal (&rest args) - (change-inflation-range 0.55) - (switch-global-planner-observation t) ;;enable tilt scan - t) - -(defun check-target-floor (userdata) - (let ((topic (instance ros::ros-callback-message :init roseus::StringStamped "/elevator_number/result")) - (target-floor (cdr (assoc 'target-floor userdata)))) - (dotimes (i 300 nil) - (ros::spin-once) - (if (and (send topic :msg) - (substringp (concatenate string "/" target-floor) - (send topic :msg :data))) - (return-from check-target-floor t) - (ros::ros-info "waiting /elevator_number/result")) - (unix::usleep (* 500 1000)) - ))) - -(make-attention-action ready-to-outside (userdata) - (clear-costmap) - (pr2-tuckarm-pose) - (send *ri* :go-pos 0 0 90) - - (update-robot-position) - ;; look at panel - (let ((look-target - (or (find-if #'(lambda(x)(string= (send x :name) "/elevator_number")) (send *panel* :descendants)) *panel*))) - (send *pr2* :head :look-at (send look-target :worldpos)) - (send *pr2* :head :look-at (send look-target :worldpos)) ;; bug twice - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) - (send *ri* :wait-interpolation)) - - (unix::sleep 3) ;; this is pr2 reference coords problem -> robot - (check-target-floor userdata) - - (send *pr2* :head :angle-vector #f(0 0)) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) - - (let ((targetfloor (cdr (assoc 'target-floor userdata)))) - (dolist (floor *floors*) - (when (equal targetfloor (elt floor 0)) - (speak-jp (concatenate string (elt floor 1) "につきました")) - (change-floor (format nil "~A/~a" (send *scene* :name) - (string-downcase (elt floor 0)))) - ))) - (clear-costmap) - :success) - -;; elevator-front-coords, front-coords, outside-panel-name, outside-button, inside-coords, inside-panel-name, target-floor, outside-coords -(defun elevator-state-machine () - (let ((sm (instance state-machine :init)) - (move-client (pr2-interface-move-base-action *ri*))) - (send sm :add-node (actionlib-client-state - :move-front move-client :timeout 300 :retry t - :key 'coords->movebaseactiongoal)) - (send (send sm :node :move-front) :remap-list '((:goal . front-coords))) - (send sm :add-node (instance state :init :costmap-for-elevator 'costmap-for-elevator)) - (send sm :add-node (instance state :init :push-outside (push-state-machine) - :remap-list '((panel-name . outside-panel-name) (button . outside-button)))) -;; (send sm :add-node (instance state :init :check-elevator-empty 'check-until-elevator-empty)) - (send sm :add-node (instance state :init :speak-fail '(lambda(x)(speak-jp "にふんたちました") t))) - ;; (send sm :add-node (actionlib-client-state - ;; :move-inside move-client :timeout 120 :retry t - ;; :key 'coords->movebaseactiongoal)) - ;; (send (send sm :node :move-inside) :remap-list '((:goal . inside-coords))) - - ;; check if elevator door open (added by furushchev 2014/04/29) - (send sm :add-node (instance state :init :move-inside (move-inside-state-machine) - :remap-list '((front-coords . elevator-front-coords) - (inside-coords . inside-coords)))) - (send sm :add-node (instance state :init :push-inside (push-state-machine) - :remap-list '((panel-name . inside-panel-name) (button . target-floor)))) - (send sm :add-node (instance state :init :ready-to-outside 'ready-to-outside)) - (send sm :add-node (actionlib-client-state - :move-outside move-client :timeout 60 :retry t - :key 'coords->movebaseactiongoal)) - (send (send sm :node :move-outside) :remap-list '((:goal . outside-coords))) - (send sm :add-node (instance state :init :costmap-normal 'costmap-normal)) - (send sm :arg-keys 'elevator-front-coords 'front-coords 'outside-panel-name 'outside-button 'inside-coords - 'inside-panel-name 'target-floor 'outside-coords :cancel) - (send sm :goal-state (list :success :fail-outside :fail-inside)) - (send sm :start-state :move-front) - ;; - (send sm :add-transition :move-front :costmap-for-elevator :succeeded) - (send sm :add-transition :move-front :fail-outside :failed) - (send sm :add-transition :costmap-for-elevator :push-outside t) - (send sm :add-transition :push-outside :move-inside :success) - (send sm :add-transition :move-inside :push-inside :success) - (send sm :add-transition :move-inside :speak-fail :fail) - (send sm :add-transition :push-outside :push-outside :fail) - (send sm :add-transition :speak-fail :move-front t) - (send sm :add-transition :push-inside :ready-to-outside :success) - (send sm :add-transition :push-inside :push-inside :fail) - (send sm :add-transition :ready-to-outside :move-outside :success) - (send sm :add-transition :move-outside :costmap-normal :succeeded) - (send sm :add-transition :move-outside :fail-inside :failed) - (send sm :add-transition :costmap-normal :success t) - sm )) - -;; elevator_move_base action server -;; target_pose :frame_id = "/map" -> convert pose to each floor local -(defun elevator-smach-initial (userdata) ;; goal -> args - (setq *logger* (instance mongo-logger :init "elevator")) - (update-robot-position) - (let* ((goal (cdr (assoc :goal userdata))) - (goal-pose (ros::tf-pose-stamped->coords - (send goal :target_pose))) - (cur-floor (check-current-floor)) - (target-floor (check-current-floor goal-pose)) target-floor-button - (target-coords (transform-pose-to-target-frame goal-pose (send *scene* :name))) - (up/down (cond ((send *scene* :floor< target-floor cur-floor) "down") - ((send *scene* :floor< cur-floor target-floor) "up") - (t nil)))) - (print (list cur-floor '-> target-floor up/down target-coords)) - (setq target-floor-button - (elt (assoc target-floor *floors* :test #'string= - :key #'(lambda(x)(format nil "~A/~A" (send *scene* :name) - (string-downcase (elt x 0))))) 0)) ;; /eng8/1f -> 1F - (set-alist 'inside-panel-name "/elevator_inside_panel" userdata) - (set-alist 'outside-panel-name "/elevator_call_panel" userdata) - ;; transform to scene frame - (set-alist 'elevator-front-coords - (let ((coords (send *scene* :transformation (car (send *scene* :find-object (format nil "~A/elevator-outside" cur-floor)))))) - (send coords :name (send *scene* :name)) - (send coords :rotate pi :z) - coords) userdata) - (set-alist 'front-coords (let ((coords (send *scene* :transformation (car (send *scene* :find-object (format nil "~A/elevator_call_panel-front" cur-floor)))))) (send coords :name (send *scene* :name)) coords) userdata) - (set-alist 'inside-coords (let ((coords (send *scene* :transformation (car (send *scene* :find-object (format nil "~A/elevator_inside_panel-front" cur-floor)))))) (send coords :name (send *scene* :name)) coords) userdata) - (set-alist 'outside-coords (let ((coords (send *scene* :transformation (car (send *scene* :find-object (format nil "~A/elevator-outside" target-floor)))))) (send coords :name (send *scene* :name)) coords) userdata) - (set-alist 'target-floor target-floor-button userdata) ;; only for elevator - (set-alist 'outside-button up/down userdata) - (set-alist 'target-coords target-coords userdata) - (print (list cur-floor '-> target-floor up/down target-coords)) - (pr2-tuckarm-pose) ;; temporary, safety pose - (not up/down))) - -(defun elevator-smach () - (let ((sm (instance state-machine :init))) - (send sm :add-node (instance state :init :initial 'elevator-smach-initial)) - (send sm :add-node (instance state :init :speak-elevator '(lambda (x) (costmap-normal) (speak-jp "えれべーたにのります") t))) - (send sm :add-node (instance state :init :take-elevator (elevator-state-machine))) - (send sm :add-node (actionlib-client-state - :go-to (pr2-interface-move-base-action *ri*) - :timeout 600 :retry t - :key 'coords->movebaseactiongoal)) - (send (send sm :node :go-to) :remap-list '((:goal . target-coords))) - (send sm :arg-keys 'elevator-front-coords 'front-coords 'outside-panel-name 'outside-button 'inside-coords - 'inside-panel-name 'target-floor 'outside-coords 'target-coords :goal :cancel) - (send sm :goal-state (list :success :fail)) - (send sm :start-state :initial) - (send sm :add-transition :initial :go-to t) - (send sm :add-transition :initial :speak-elevator nil) - (send sm :add-transition :speak-elevator :take-elevator t) - (send sm :add-transition :take-elevator :go-to :success) - (send sm :add-transition :take-elevator :fail :fail-inside) - (send sm :add-transition :take-elevator :fail :fail-outside) - (send sm :add-transition :go-to :success :succeeded) - (send sm :add-transition :go-to :fail :failed) - sm )) - diff --git a/elevator_move_base_pr2/src/elevator-move-base.l b/elevator_move_base_pr2/src/elevator-move-base.l index 2a4581e1b3..84e07bfc72 100755 --- a/elevator_move_base_pr2/src/elevator-move-base.l +++ b/elevator_move_base_pr2/src/elevator-move-base.l @@ -1,34 +1,55 @@ #!/usr/bin/env roseus -(ros::roseus "elevator_move_base") +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "move_base_msgs") +(ros::roseus-add-srvs "nav_msgs") +(ros::roseus-add-srvs "std_srvs") + +(require :state-machine-actionlib + "package://roseus_smach/src/state-machine-actionlib.l") + +(load "package://elevator_move_base_pr2/src/state-machine-main.l") +(load "package://elevator_move_base_pr2/src/utils.l") + +(defvar *sm*) -(load "package://elevator_move_base_pr2/src/elevator-move-base-main.l") (defun simple-goal-cb (msg) (let ((pub-msg (instance move_base_msgs::MoveBaseActionGoal :init)) - (stamp (ros::time-now))) + (stamp (ros::time-now))) (send pub-msg :header :stamp stamp) (send pub-msg :goal_id :stamp stamp) (send pub-msg :goal_id :id (format nil "~A" (send stamp :sec-nsec))) (send pub-msg :goal :target_pose msg) - (clear-costmap) - (ros::publish "/elevator_move_base/goal" pub-msg) - )) + (ros::publish "/elevator_move_base/goal" pub-msg))) + + +(ros::roseus "elevator_move_base") -;; (initialize-env) -(ros::subscribe "/elevator_move_base_simple/goal" geometry_msgs::PoseStamped - #'simple-goal-cb) +(ros::advertise "view_target" geometry_msgs::PointStamped 1) (ros::advertise "/elevator_move_base/goal" move_base_msgs::MoveBaseActionGoal 1) +(unix:usleep (* 100 1000)) ;; Wait for setting up publisher +(ros::subscribe "/elevator_move_base_simple/goal" geometry_msgs::PoseStamped + #'simple-goal-cb) -(setq sm (elevator-smach)) -(setq s (instance state-machine-action-server :init "/elevator_move_base" - move_base_msgs::MoveBaseAction sm)) -(send s :success-state '(:success)) ;; name of success state +;; Relay services +(ros::advertise-service + "/elevator_move_base/make_plan" nav_msgs::GetPlan + #'(lambda (req) (call-service "/move_base_node/make_plan" req))) +(ros::advertise-service + "/elevator_move_base/clear_costmaps" std_srvs::Empty + #'(lambda (req) (call-service "/move_base/clear_costmaps" req))) + +(setq *sm* (instance state-machine-action-server :init "/elevator_move_base" + move_base_msgs::MoveBaseAction + (elevator-move-base-state-machine) + '((push-arm . :larm)))) +(send *sm* :success-state '(:succeed)) ;; name of success state (ros::ros-info "initialized elevator-move-base.l") -(ros::rate 10) -(do-until-key - (ros::spin-once) - (send s :worker) - (ros::sleep)) +(ros::rate 10) +(while (ros::ok) + (ros::spin-once) + (send *sm* :worker) + (ros::sleep)) diff --git a/elevator_move_base_pr2/src/eng2-through-door-nav.l b/elevator_move_base_pr2/src/eng2-through-door-nav.l deleted file mode 100644 index 9596f283c6..0000000000 --- a/elevator_move_base_pr2/src/eng2-through-door-nav.l +++ /dev/null @@ -1,24 +0,0 @@ -(defun move-to-through-door-eng2-2f (userdata) - (let ((co (cdr (assoc :goal userdata))) - cur co-2f d1 d2 ret (thr 10500)) - (setq cur (send (send *ri* :state :worldcoords "eng2/2f") :worldpos)) - (setq co-2f (send - (send *tfl* :lookup-transform "eng2/2f" "world" (ros::time 0)) - :transform co)) - (when (and (string= (send *scene* :name) "eng2") - (string= (send *scene* :current-floor co) "eng2/2f") - (or (< (elt (send co-2f :worldpos) 0) thr (elt cur 0)) - (< (elt cur 0) thr (elt (send co-2f :worldpos) 0)))) - (setq d1 (make-coords :pos (float-vector (- thr 750) -27000 0)) - d2 (make-coords :pos (float-vector (+ thr 1500) -27000 0))) - (when (< (elt (send co-2f :worldpos) 0) thr (elt cur 0)) - (send d1 :rotate pi :z) (send d2 :rotate pi :z) - (setq tmp d1 d1 d2 d2 tmp)) ;; swap - (when (not (send *ri* :move-to d1 :frame-id "eng2/2f")) - (return-from move-to-through-door-eng2-2f :failed)) - (switch-global-planner-observation nil) ;;disable tilt scan - (setq ret (send *ri* :move-to d2 :frame-id "eng2/2f")) - (switch-global-planner-observation t) ;;enable tilt scan - (when (not ret) (return-from move-to-through-door-eng2-2f :failed))) - (send *ri* :move-to co) - :succeeded)) diff --git a/elevator_move_base_pr2/src/find-elevator-button.l b/elevator_move_base_pr2/src/find-elevator-button.l index 7e127babb1..143af0b784 100755 --- a/elevator_move_base_pr2/src/find-elevator-button.l +++ b/elevator_move_base_pr2/src/find-elevator-button.l @@ -1,56 +1,21 @@ #!/usr/bin/env roseus -(ros::load-ros-manifest "elevator_move_base_pr2") - -(ros::roseus "find_elevator_button") - (load "package://elevator_move_base_pr2/src/posedetectiondb-client.l") -(load "package://elevator_move_base_pr2/src/elevator-buttons.l") +(load "package://elevator_move_base_pr2/src/utils.l") -(load "package://pr2eus/pr2-interface.l") -(defvar *tfl* (instance ros::transform-listener :init)) -(unix::sleep 1) ;; wait for TF messages +(defvar *clients*) +(defvar *scene*) -(let ((frames (send *tfl* :get-frame-strings))) - (cond ((find "eng2" frames :test #'string=) - (setq *scene* (make-eng2-scene))) - ((find "eng8" frames :test #'string=) - (setq *scene* (make-eng8-scene))) - (t - (ros::ros-error "There is no building frame-id in TF tree exiting...") - (exit)) - )) -(let ((objs (list (send *scene* :object "/elevator_inside_panel") - (send *scene* :object "/elevator_call_panel")))) - (setq *clients* (init-posedetectiondb-client objs))) -;;(setq *ri* (instance pr2-interface :init)) -(defun display-available-p () - (and x::*display* (> x::*display* 0))) +(ros::roseus "find_elevator_button") -(setq *node-debug* nil) -(when *node-debug* - (setq *ri* (instance pr2-interface :init)) - (setq *pr2* (pr2)) - (when (display-available-p) - (if (not (boundp '*irtviewer*)) (make-irtviewer :title "find-elevator-button")) - (objects (append (list *pr2*) (send *scene* :objects))) - (send *irtviewer* :look-all *pr2*)) - ) +(setq *scene* (make-scene-by-tf (instance ros::transform-listener :init))) +(setq *clients* (init-posedetectiondb-client + (list (send *scene* :object "/elevator_inside_panel") + (send *scene* :object "/elevator_call_panel")))) (ros::rate 10) (while (ros::ok) - (ros::spin-once) - (when *node-debug* - (send *pr2* :angle-vector (send *ri* :state)) - (when (display-available-p) - (send *irtviewer* :draw-objects) - (let ((camera-coords (send *tfl* :lookup-transform "base_footprint" "narrow_stereo_optical_frame" (ros::time 0)))) - (when camera-coords - (send camera-coords :draw-on :flush t :size 200))) - (x::window-main-one)) - ) - (ros::sleep) - ) - + (ros::spin-once) + (ros::sleep)) diff --git a/elevator_move_base_pr2/src/matchtemplate.py b/elevator_move_base_pr2/src/matchtemplate.py deleted file mode 100755 index 3914e6f1cc..0000000000 --- a/elevator_move_base_pr2/src/matchtemplate.py +++ /dev/null @@ -1,177 +0,0 @@ -#!/usr/bin/env python - -import roslib -import rospy -from sensor_msgs.msg import Image -from roseus.msg import StringStamped -import cv -from cv_bridge import CvBridge, CvBridgeError - -bridge = CvBridge() -latest_msg = None -image_sub = None -debug_pub = None - -# for cv_bridge back compatibility -def encoding_as_cvtype(encoding): - from cv_bridge.boost.cv_bridge_boost import getCvType - try: - return getCvType(encoding) - except RuntimeError as e: - raise CvBridgeError(e) - - -def imgmsg_to_cv(img_msg, desired_encoding = "passthrough"): - try: - return bridge.imgmsg_to_cv(img_msg, desired_encoding) - except: - cv2_im = bridge.imgmsg_to_cv2(img_msg, desired_encoding) - img_msg = bridge.cv2_to_imgmsg(cv2_im) - source_type = encoding_as_cvtype(img_msg.encoding) - im = cv.CreateMatHeader(img_msg.height, img_msg.width, source_type) - cv.SetData(im, img_msg.data, img_msg.step) - return im - - -def cv_to_imgmsg(cvim, encoding = "passthrough"): - try: - return bridge.cv_to_imgmsg(cvim, encoding) - except: - img_msg = Image() - (img_msg.width, img_msg.height) = cv.GetSize(cvim) - if encoding == "passthrough": - img_msg.encoding = bridge.cvtype_to_name[cv.GetElemType(cvim)] - else: - img_msg.encoding = encoding - if encoding_as_cvtype(encoding) != cv.GetElemType(cvim): - raise CvBridgeError, "invalid encoding" - img_msg.data = cvim.tostring() - img_msg.step = len(img_msg.data) / img_msg.height - return img_msg - - -def image_callback (msg): - global latest_msg - latest_msg = msg - return - -def process_msg (): - global latest_msg - msg = latest_msg - latest_msg = None - if msg == None: return - - result = StringStamped(data='',header=msg.header) - - try: - cv_image = imgmsg_to_cv(msg, "mono8") - except CvBridgeError, e: - print e - - reslist = [] - for template in templates: - temptype = template[0] - tempimg = template[1] - tempthre = template[2] - tempname = template[3] - if template[4] == 'CCORR': tempmethod = cv.CV_TM_CCORR_NORMED - elif template[4] == 'CCOEFF': tempmethod = cv.CV_TM_CCOEFF_NORMED - else: tempmethod = cv.CV_TM_SQDIFF_NORMED - ressize = list(cv.GetSize(cv_image)) - ressize[0] -= cv.GetSize(tempimg)[0] - 1 - ressize[1] -= cv.GetSize(tempimg)[1] - 1 - results = cv.CreateImage(ressize, cv.IPL_DEPTH_32F, 1 ) - cv.MatchTemplate(cv_image, tempimg, results, tempmethod) - - status = cv.MinMaxLoc(results) - if (tempmethod == cv.CV_TM_SQDIFF_NORMED): - found = ( status[0] < tempthre ) - reslist += [(tempname,(status[0],status[2],tempthre,found))] - if found : - result.data += tempname+' ' - else : - found = (tempthre < status[1] ) - reslist += [(tempname,(status[1],status[3],tempthre,found))] - if found : - result.data += tempname+' ' - print reslist - - result_pub.publish(result) - publish_debug(cv_image, reslist) - -def publish_debug(img, results): - imgsize = cv.GetSize(img) - sizelist = [cv.GetSize(tmp[1]) for tmp in templates] - width = max(imgsize[0], sum([s[0] for s in sizelist])) - height = imgsize[1] + max([s[1] for s in sizelist]) - output = cv.CreateImage((width, height), cv.IPL_DEPTH_8U, 1) - cv.Zero(output) - cur_x = 0 - - view_rect = (0, height-imgsize[1], imgsize[0], imgsize[1]) - cv.SetImageROI(output, view_rect) - cv.Copy(img, output) - for template in templates: - size = cv.GetSize(template[1]) - cv.SetImageROI(output, (cur_x, 0, size[0], size[1])) - cv.Copy(template[1], output) - cur_x += size[0] - - #cv.PutText(output, tempname, (0,size[1]-16), font, cv.CV_RGB(255,255,255)) - #cv.PutText(output, str(tempthre)+'<'+str(status[1]), (0,size[1]-8), font, cv.CV_RGB(255,255,255)) - for _,status in [s for s in results if s[0] == template[3]]: - print status - cv.PutText(output, template[3], (0,size[1]-42), font, cv.CV_RGB(255,255,255)) - cv.PutText(output, "%7.5f"%(status[0]), (0,size[1]-24), font, cv.CV_RGB(255,255,255)) - cv.PutText(output, "%7.5f"%(status[2]), (0,size[1]-8), font, cv.CV_RGB(255,255,255)) - if status[3] : - cv.Rectangle(output, (0, 0), size, cv.RGB(255,255,255), 9) - cv.SetImageROI(output, view_rect) - for _,status in [s for s in results if s[0] == template[3]]: - pt2 = (status[1][0]+size[0], status[1][1]+size[1]) - if status[3] : - cv.Rectangle(output, status[1], pt2, cv.RGB(255,255,255), 5) - - cv.ResetImageROI(output) - debug_pub.publish(cv_to_imgmsg(output, encoding="passthrough")) - -class MySubscribeListener(rospy.SubscribeListener): - def peer_subscribe(self, topic_name, topic_publish, peer_publish): - global image_sub - if image_sub is None: - image_sub = rospy.Subscriber(rospy.resolve_name("~image"),Image,image_callback,queue_size=1) - def peer_unsubscribe(self, topic_name, num_peers): - global image_sub - if num_peers == 0: - image_sub.unregister() - image_sub = None - -def resolve_ros_path(path): - if "package://" in path: - plist = path.split('/') - pkg_dir = roslib.packages.get_pkg_dir(plist[2]) - return "/".join([pkg_dir] + plist[3:]) - return path - -if __name__=='__main__': - rospy.init_node('match_template') - - global templates, result_pub, font - font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.5, 0.5, 0.0, 1, cv.CV_AA) - template_list = rospy.get_param('~template_list').split() - templates = [[typename,cv.LoadImage(resolve_ros_path(rospy.get_param('~template/'+typename+'/path')),cv.CV_LOAD_IMAGE_GRAYSCALE),rospy.get_param('~template/'+typename+'/thre'),rospy.get_param('~template/'+typename+'/name',''),rospy.get_param('~template/'+typename+'/method','')] for typename in template_list] - - result_pub = rospy.Publisher("~result", StringStamped, MySubscribeListener(), queue_size=1) - debug_pub = rospy.Publisher("~debug_image", Image, queue_size=1) - - try: - while not rospy.is_shutdown(): - process_msg() - rospy.sleep(0.1) - except rospy.ROSInterruptException: pass - - result_pub.unregister() - debug_pub.unregister() - if image_sub != None: - image_sub.unregister() - diff --git a/elevator_move_base_pr2/src/move-inside-elevator.l b/elevator_move_base_pr2/src/move-inside-elevator.l deleted file mode 100644 index dadead4012..0000000000 --- a/elevator_move_base_pr2/src/move-inside-elevator.l +++ /dev/null @@ -1,45 +0,0 @@ -(setq *change-publisher-topic* "/pcl_nodelet/octree_change_detector/octree_change_result") -(setq *change-pc* nil) -(setq *change-threshold* (ros::get-param "change_threshold" 200)) - -(defun move-inside-state-machine () - (let ((sm (instance state-machine :init)) - (move-client (pr2-interface-move-base-action *ri*))) - (send sm :add-node (instance state :init :tuckarm-pose '(lambda (&optional args) (pr2-tuckarm-pose)))) - (send sm :add-node (actionlib-client-state - :move-elevator-front move-client :timeout 300 :retry t - :key 'coords->movebaseactiongoal)) - (send (send sm :node :move-elevator-front) :remap-list '((:goal . front-coords))) - (send sm :add-node (instance state :init :check-elevator-open 'check-elevator-open)) - (send sm :add-node (actionlib-client-state - :move-elevator-inside move-client :timeout 300 :retry t - :key 'coords->movebaseactiongoal)) - (send (send sm :node :move-elevator-inside) :remap-list '((:goal . inside-coords))) - (send sm :arg-keys 'front-coords 'inside-coords) - (send sm :goal-state (list :success :fail)) - (send sm :start-state :tuckarm-pose) - (send sm :add-transition :tuckarm-pose :move-elevator-front t) - (send sm :add-transition :move-elevator-front :check-elevator-open :succeeded) - (send sm :add-transition :move-elevator-front :fail :failed) - (send sm :add-transition :check-elevator-open :move-elevator-inside t) - (send sm :add-transition :check-elevator-open :fail nil) - (send sm :add-transition :move-elevator-inside :success :succeeded) - (send sm :add-transition :move-elevator-inside :fail :failed) - sm)) - -(defun pc-cb (msg) - (setq *change-pc* (* (send msg :height) (send msg :width))) - (ros::ros-info "change: ~A" *change-pc*)) - -(defun check-elevator-open (&optional args) - (ros::subscribe *change-publisher-topic* sensor_msgs::PointCloud2 #'pc-cb) - (let ((start-time (ros::time-now))) - (while (or - (not *change-pc*) - (< *change-pc* *change-threshold*)) - (ros::ros-warn "change: ~A" *change-pc*) - (ros::spin-once) - (ros::sleep))) - (ros::unsubscribe *change-publisher-topic*) - (clear-costmap) - t) diff --git a/elevator_move_base_pr2/src/navigation-client.l b/elevator_move_base_pr2/src/navigation-client.l deleted file mode 100644 index 0a9102277d..0000000000 --- a/elevator_move_base_pr2/src/navigation-client.l +++ /dev/null @@ -1,42 +0,0 @@ - -(ros::roseus-add-msgs "geometry_msgs") - -(defun load-coords-from-db (db table parent child) - (car (select-tf-from-table db :frame_id parent :child_id child :table table :limit 1))) - -(defun transform-pose-to-target-frame (coords frame-id &optional (tm (ros::time 0))) ;; (send coords :name) is frame-if - (let ((tra (send *tfl* :lookup-transform frame-id (send coords :name) tm))) - (when tra - (send tra :transform coords) - (send tra :name frame-id) - tra))) - -(defun check-current-floor (&optional pose) ;; :name == frame_id - (if pose - (setq pose (transform-pose-to-target-frame pose (send *scene* :name))) - (setq pose (send *tfl* :lookup-transform (send *scene* :name) "base_footprint" (ros::time 0)))) - (send *scene* :current-floor pose)) - -(defun change-floor (target-floor &optional (topicname "/initialpose3d")) - (ros::advertise topicname geometry_msgs::PoseWithCovarianceStamped 1) - (unix:sleep 1);; important - (let* ((msg (instance geometry_msgs::PoseWithCovarianceStamped :init)) - (current-floor-frame (check-current-floor)) - (robot-pose (send *tfl* :lookup-transform current-floor-frame "base_footprint" (ros::time 0)))) - (if (null robot-pose) - (return-from change-floor nil)) - (send msg :header :frame_id target-floor) - (send msg :header :stamp (ros::time-now)) - (let ((cov (send msg :pose :covariance))) - (setf (elt cov 0) 0.05) - (setf (elt cov 7) 0.05) - (setf (elt cov 21) 0.02)) - (send msg :pose :pose (ros::coords->tf-pose robot-pose)) - (ros::publish topicname msg))) - -;; -;; for dynamic configuration of costmap params -;; -;; reset local costmap and clear unknown grid around robot -(defun switch-global-planner-observation (enable) - (use-tilt-laser-obstacle-cloud enable)) diff --git a/elevator_move_base_pr2/src/posedetectiondb-client.l b/elevator_move_base_pr2/src/posedetectiondb-client.l index 381a373ada..fa0435cd78 100644 --- a/elevator_move_base_pr2/src/posedetectiondb-client.l +++ b/elevator_move_base_pr2/src/posedetectiondb-client.l @@ -1,41 +1,47 @@ -(load "ros-callback-manager.l") -;;(load "add-msg-pgsql.l") - -;; posedetectiondb -> tf client class -;;(load "package://euslib/jsk/kalmanlib.l") +(ros::roseus-add-msgs "geometry_msgs") (ros::roseus-add-msgs "posedetection_msgs") + +(load "package://elevator_move_base_pr2/src/ros-callback-manager.l") + + ;; get vector in vlist , that is least median distance -(defun lmeds-vector - (vlist) - (find-extream vlist - #'(lambda(p)(let ((diffs (mapcar #'(lambda(p1)(distance p p1)) vlist))) - (elt (sort diffs #'<) (/ (length diffs) 2)))) - #'<=)) -(defun lmeds-pose - (pose-list) +(defun lmeds-vector (vlist) + (find-extream + vlist + #'(lambda (p) (let ((diffs (mapcar #'(lambda (p1) (distance p p1)) vlist))) + (elt (sort diffs #'<) (/ (length diffs) 2)))) + #'<=)) + +(defun lmeds-pose (pose-list) (make-coords :pos (lmeds-vector (send-all pose-list :pos)) - :rot (quaternion2matrix (lmeds-vector (mapcar #'matrix2quaternion (send-all pose-list :rot)))))) + :rot (quaternion2matrix + (lmeds-vector (mapcar #'matrix2quaternion + (send-all pose-list :rot)))))) + +;; posedetectiondb -> tf client class (defclass posedetectiondb-client - :super ros::ros-callback-object - :slots (object time-stamp poselist frame-id fixed-frame filter-type)) + :super ros-callback-object + :slots (objects prev-tm poselist fixed-frame filter-type tfl tfb)) + (defmethod posedetectiondb-client - (:init - (obj &key (topic "ObjectDetection") ((:object-frame _obj) nil) - ((:fixed-frame _fixed) "/base_footprint") ((:filter-type ft) nil)) - (setq object obj frame-id _obj fixed-frame _fixed filter-type ft) - (setq time-stamp (ros::time 0)) - (unless (boundp '*tfl*) - (defvar *tfl* (instance ros::transform-listener :init))) - (unless (boundp '*tfb*) - (defvar *tfb* (instance ros::transform-broadcaster :init))) + (:init (objs + &key (topic "ObjectDetection") + ((:fixed-frame _fixed) "/base_footprint") + ((:filter-type ft) :lmeds)) + (setq objects objs) + (setq fixed-frame _fixed) + (setq filter-type ft) + (setq prev-tm (ros::time 0)) + (setq tfl (instance ros::transform-listener :init)) + (setq tfb (instance ros::transform-broadcaster :init)) (send-super :init topic posedetection_msgs::ObjectDetection :detection-cb)) - ;; filter detection pose, and move obj to the estimated coords - (:update-coords - (coords &optional (relative :local)) - (when (< 5 (send (ros::time- (ros::time-now) time-stamp) :to-sec)) + + (:update-coords (coords &optional (relative :local)) + "Filter detection pose, and move obj to the estimated coords" + (when (< 5 (send (ros::time- (ros::time-now) prev-tm) :to-sec)) (setq poselist nil)) - (setq time-stamp (ros::time-now)) + (setq prev-tm (ros::time-now)) (when (not (equal coords (car poselist))) (push (send coords :copy-worldcoords) poselist) (when (< 20 (length poselist)) @@ -43,41 +49,43 @@ (pop poselist) (nreverse poselist))) (case filter-type - (:lmeads - (send object :move-to (lmeds-pose poselist) relative)) - (t - (send object :move-to coords relative)))) - (:detection-cb - (msg) + (:lmeds + (send object :move-to (lmeds-pose poselist) relative)) + (t + (send object :move-to coords relative)))) + + (:detection-cb (msg) (dolist (object-msg (send msg :objects)) - (when (equal (string-left-trim "/" (send object-msg :type)) - (string-left-trim "/" (or frame-id (send object :name)))) - ;; broadcast transform - (send *tfl* :wait-for-transform - fixed-frame (send msg :header :frame_id) - (send msg :header :stamp) 1) - (let* ((pose (instance geometry_msgs::PoseStamped :init - :header (send msg :header) - :pose (send object-msg :pose))) - (coords (ros::tf-pose->coords (send pose :pose))) - (objcoords (send *tfl* :transform-pose - fixed-frame pose))) - (ros::ros-info "posedetection-cb type:~A, coords~A" (send object-msg :type) coords) - (when objcoords - (send self :update-coords objcoords :world) - (putprop object (send msg :header :stamp) :stamp) - (send *tfb* :send-transform object - fixed-frame (or frame-id (send object :name)) - (send msg :header :stamp))) - )))) + (dolist (object objects) + (when (substringp (string-left-trim "/" (send object :name)) + (string-left-trim "/" (send object-msg :type))) + ;; broadcast transform + (send tfl :wait-for-transform + fixed-frame (send msg :header :frame_id) + (send msg :header :stamp) 1) + (let* ((pose (instance geometry_msgs::PoseStamped :init + :header (send msg :header) + :pose (send object-msg :pose))) + (coords (ros::tf-pose->coords (send pose :pose))) + (objcoords (send tfl :transform-pose + fixed-frame pose))) + (ros::ros-info "posedetection-cb type: ~A, coords: ~A" + (send object-msg :type) coords) + (when objcoords + (send self :update-coords objcoords :world) + (putprop object (send msg :header :stamp) :stamp) + (send tfb :send-transform object + fixed-frame (send object :name) + (send msg :header :stamp))) + ))))) ) + (defun init-posedetectiondb-client (objects &key topic) - (let (clients) - (dolist (obj objects) - (push - (if topic (instance posedetectiondb-client :init obj :topic topic) - (instance posedetectiondb-client :init obj)) - clients)) - (ros::ros-info "initialize posedetection db client with ~A~%" (send-all objects :name)) + (let ((clients + (if topic + (instance posedetectiondb-client :init objects :topic topic) + (instance posedetectiondb-client :init objects)))) + (ros::ros-info "initialize posedetection db client with ~A~%" + (send-all objects :name)) clients)) diff --git a/elevator_move_base_pr2/src/push-elevator-button.l b/elevator_move_base_pr2/src/push-elevator-button.l deleted file mode 100644 index 457931f503..0000000000 --- a/elevator_move_base_pr2/src/push-elevator-button.l +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env runeus - -(load "elevator-buttons.l") -(require :pr2-interface "package://pr2eus/pr2-interface.l") -;(load "package://manipserver_lib/euslisp/manip_client_lib.l") - -(defun push-button (target-coords) - (let (rayvec via-coords (arm :rarm) - via-angle-vector org-arm-anglev - (rarm-reset #f(-60 70 110 -120 160 -30 180)) ;; second element 80 -> 70 - (larm-reset #f(60 70 70 -120 -160 -30 180))) ;; second element 80 -> 70 - ;; check push target simply from current tuckarm-pose - (ros::spin-once) - (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) - ;; if the free-arm is grasping something // TODO: smart arm change motion - (format t "grasp -> ~A~%" (send *ri* :start-grasp :arms)) - (when (< 5 (send *ri* :start-grasp :larm)) - (pr2-tuckarm-pose :rarm)) - (when (< 5 (send *ri* :start-grasp :rarm)) - (pr2-tuckarm-pose :larm)) - - (setq arm (check-tuckarm-pose)) - (setq rayvec (normalize-vector (v- (send *pr2* :narrow_stereo_optical_frame_lk :worldpos) (send target-coords :worldpos)))) - - ;; via-coords - (setq via-coords (send (send target-coords :copy-worldcoords) - :translate (scale 100 rayvec) :world)) - ;; push 30mm - (setq target-coords (send (send target-coords :copy-worldcoords) - :translate (scale -30 rayvec) :world)) - - (when arm - (send *pr2* arm :angle-vector (case arm (:rarm rarm-reset) - (:larm larm-reset))) - ;; check IK for free-arm is solvable - (unless (and arm (send *pr2* arm :inverse-kinematics target-coords :rotation-axis :x)) - (setq *ik-fail-av* (send *pr2* :angle-vector)) - (setq *ik-fail-coords* (send target-coords :copy-worldcoords)) - (dump-loadable-structure (format nil "ik-fail-~d.l" (send (ros::time-now) :sec)) - *ik-fail-coords* *ik-fail-av*) - (setq arm nil))) - (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) - ;; - - ;; reset-pose and select arm - (unless arm -; (setq arm (if (plusp (elt (send target-coords :worldpos) 1)) :larm :rarm)) - (setq arm (if (plusp (elt (send (send *pr2* :transformation target-coords) :worldpos) 1)) :larm :rarm)) - (send *pr2* :reset-pose) - (send *ri* :angle-vector (send *pr2* :angle-vector) 3000) - (send *ri* :wait-interpolation)) - - (ros::ros-info "select ~A arm to push button ~A" arm (send target-coords :worldpos)) - - (setq org-arm-anglev (send *pr2* arm :angle-vector)) - - ;; start push - (send *pr2* arm :angle-vector (case arm (:rarm rarm-reset) - (:larm larm-reset))) - (send *pr2* :head :look-at (send target-coords :worldpos)) - (unless - (send *pr2* arm :inverse-kinematics via-coords :rotation-axis nil) - (setq *ik-fail-av* (send *pr2* :angle-vector)) - (setq *ik-fail-coords* (send target-coords :copy-worldcoords)) - (dump-loadable-structure (format nil "ik-fail-~d.l" (send (ros::time-now) :sec)) - *ik-fail-coords* *ik-fail-av*) - ;; return to original pose - (send *pr2* arm :angle-vector org-arm-anglev) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) ;; quick! - (send *ri* :wait-interpolation) - (return-from push-button nil)) - (setq via-angle-vector (send *pr2* :angle-vector)) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1500) - (send *ri* :wait-interpolation) - - (send *pr2* arm :inverse-kinematics via-coords :rotation-axis :x - :revert-if-fail nil) - (setq via-angle-vector (send *pr2* :angle-vector)) - (send *ri* :angle-vector (send *pr2* :angle-vector) 500) - (send *ri* :wait-interpolation) - - (unless - (send *pr2* arm :inverse-kinematics target-coords :rotation-axis :x) - (setq *ik-fail-av* (send *pr2* :angle-vector)) - (setq *ik-fail-coords* (send target-coords :copy-worldcoords)) - (dump-loadable-structure (format nil "ik-fail-~d.l" (send (ros::time-now) :sec)) - *ik-fail-coords* *ik-fail-av*) - ;; return to original pose - (send *pr2* arm :angle-vector org-arm-anglev) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) ;; quick! - (send *ri* :wait-interpolation) - (return-from push-button nil)) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) - (send *ri* :wait-interpolation) - - (send *ri* :angle-vector via-angle-vector 400) - (send *ri* :wait-interpolation) - - ;; return original pose - ;;(send *pr2* :head :angle-vector #f(0 0)) - (send *pr2* arm :angle-vector org-arm-anglev) - (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) ;; quick! - (send *ri* :wait-interpolation) -#| - (send *s* :arm-target-coords arm via-coords) - (unix:sleep 10) - (send *s* :arm-target-coords arm target-coords) - (unix:sleep 2) - (send *s* :arm-target-coords arm via-coords) -|# - t)) - diff --git a/elevator_move_base_pr2/src/ros-callback-manager.l b/elevator_move_base_pr2/src/ros-callback-manager.l index 71b75a1c6b..b573eaf090 100644 --- a/elevator_move_base_pr2/src/ros-callback-manager.l +++ b/elevator_move_base_pr2/src/ros-callback-manager.l @@ -1,62 +1,64 @@ -;;; -;;; Manager Class -;;; - -(defclass ros::ros-callback-manager +(defclass ros-callback-manager :super propertied-object :slots (subscribed-topics)) -(defmethod ros::ros-callback-manager - (:init () ) - (:add-subscribe - (ros-object topic-name topic-type callback) + +(defmethod ros-callback-manager + (:init ()) + + (:add-subscribe (ros-object topic-name topic-type callback) (unless (member topic-name subscribed-topics :key #'car :test #'equal) - (ros::ros-info "subscribe ~A ~A" topic-name topic-type) + (ros::ros-info "Start subscribing topic: ~A, type: ~A" + topic-name topic-type) (ros::subscribe topic-name topic-type #'send self :callback topic-name)) (push (list topic-name ros-object callback) subscribed-topics)) + (:delete-subscribe (obj topic-name) - (setq subscribed-topics - (delete-if #'(lambda(x)(and (eq (cadr x) obj) (string= (car x) topic-name))) - subscribed-topics)) + (setq subscribed-topics + (delete-if #'(lambda (x) (and (eq (cadr x) obj) + (string= (car x) topic-name))) + subscribed-topics)) (unless (member topic-name subscribed-topics :key #'car :test #'equal) - (ros::ros-info "unsubscribe ~A" topic-name) + (ros::ros-info "Unsubscribing ~A" topic-name) (ros::unsubscribe topic-name))) - (:callback - (topic-name msg) + + (:callback (topic-name msg) (dolist (subscribed-topic subscribed-topics) (when (string= topic-name (car subscribed-topic)) (let ((rosobj (cadr subscribed-topic)) - (callback (caddr subscribed-topic))) - (send rosobj callback msg))))) + (callback (caddr subscribed-topic))) + (send rosobj callback msg))))) ) -;; manager instance -(defvar ros::*ros-callback-manager* - (instance ros::ros-callback-manager :init)) -;; -;; callback super class -;; -(defclass ros::ros-callback-object +(defclass ros-callback-object :super propertied-object - :slots (topic-name)) -(defmethod ros::ros-callback-object - (:init - (name type &optional (cb :callback)) + :slots (topic-name callback-manager)) + +(defmethod ros-callback-object + (:init (name type &optional (cb :callback)) (setq topic-name name) - (send ros::*ros-callback-manager* :add-subscribe self name type cb) + (setq callback-manager (instance ros-callback-manager :init)) + (send callback-manager :add-subscribe self name type cb) self) + (:unregister () - (send ros::*ros-callback-manager* :delete-subscribe self topic-name)) + (send callback-manager :delete-subscribe self topic-name)) ) -;; sample callback class -(defclass ros::ros-callback-message - :super ros::ros-callback-object + +(defclass ros-callback-message + :super ros-callback-object :slots (msg)) -(defmethod ros::ros-callback-message + +(defmethod ros-callback-message (:init (type name) - (send-super :init name type :callback)) - (:callback (msg_) (setq msg msg_)) + (send-super :init name type :callback)) + + (:callback (msg_) + (setq msg msg_)) + (:msg (&rest args) - (if (and msg (keywordp (car args))) (send* msg args) msg)) + (if (and msg (keywordp (car args))) + (send* msg args) + msg)) ) diff --git a/elevator_move_base_pr2/src/state-machine-main.l b/elevator_move_base_pr2/src/state-machine-main.l new file mode 100644 index 0000000000..26c6ae253d --- /dev/null +++ b/elevator_move_base_pr2/src/state-machine-main.l @@ -0,0 +1,206 @@ +;; -*- coding: utf-8 -*- + +(ros::roseus-add-msgs "roseus") +(ros::roseus-add-srvs "std_srvs") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") +(require :state-machine + "package://roseus_smach/src/state-machine.l") +(require :state-machine-actionlib + "package://roseus_smach/src/state-machine-actionlib.l") + +;; (load "package://elevator_move_base_pr2/src/database-interface.l") +(load "package://elevator_move_base_pr2/src/state/check-elevator-needed.l") +(load "package://elevator_move_base_pr2/src/state/check-elevator-open.l") +(load "package://elevator_move_base_pr2/src/state/move-outside.l") +(load "package://elevator_move_base_pr2/src/state/push-elevator-button.l") +(load "package://elevator_move_base_pr2/src/state/ready-to-outside.l") +(load "package://elevator_move_base_pr2/src/state/sanity-check.l") +(load "package://elevator_move_base_pr2/src/state/speak-to-human-behind.l") +(load "package://elevator_move_base_pr2/src/utils.l") + + +(defvar *floors*) +;; (defvar *ri*) +(defvar *scene*) +(defvar *tfl*) + + +(defun initialize-env () + (pr2-init) + (setq *tfl* (instance ros::transform-listener :init)) + (setq *scene* (make-scene-by-tf *tfl*)) + (setq *floors* '(("B2F" "地下2階") ("B1F" "地下1階") + ("1F" "1階") ("2F" "2階") + ("3F" "3階") ("4F" "4階") + ("5F" "5階") ("6F" "6階") + ("7F" "7階") ("8F" "8階") + ("9F" "9階") ("10F" "10階") + ("11F" "11階") ("12F" "12階")))) + +(defun push-state-machine () + (let ((sm (instance state-machine :init))) + (send sm :add-node (instance state :init :look 'look-button-state)) + (send sm :add-node (instance state :init :push 'push-button-state)) + (send sm :add-node (instance state :init :check 'check-button-light-state)) + (send sm :arg-keys 'button 'panel-name 'push-arm) + (send sm :goal-state (list :succeed)) + (send sm :start-state :look) + (send sm :add-transition :look :push t) + (send sm :add-transition :look :look nil) + (send sm :add-transition :push :check t) + (send sm :add-transition :push :look nil) + (send sm :add-transition :check :succeed t) + (send sm :add-transition :check :look nil) + sm)) + +(defun move-inside-state-machine () + (let ((sm (instance state-machine :init)) + (move-client (pr2-interface-move-base-action *ri*))) + (send sm :add-node (instance state :init :set-param-for-elevator-outside + 'tolerance-loose)) + (send sm :add-node + (instance state :init :move-elevator-front + '(lambda (userdata) + (use-tilt-laser-obstacle-cloud nil) + (send *ri* :move-to + (cdr (assoc 'elevator-front-coords userdata)) + :retry 2 :correction nil)))) + (send sm :add-node (instance state :init :set-param-for-elevator-inside + '(lambda (x) + (tolerance-strict) + t))) + (send sm :add-node + (instance state :init :check-elevator-open + '(lambda (x) (check-elevator-open :timeout 300)))) + (send sm :add-node (instance state :init :speak-to-human-behind + 'speak-to-human-behind)) + (send sm :add-node + (instance state :init :move-elevator-inside + '(lambda (userdata) + (send *ri* :move-to + (cdr (assoc 'inside-coords userdata)) + :retry 2 :correction nil)))) + (send sm :arg-keys 'elevator-front-coords 'inside-coords) + (send sm :goal-state (list :succeed :fail)) + (send sm :start-state :set-param-for-elevator-outside) + (send sm :add-transition + :set-param-for-elevator-outside :move-elevator-front t) + (send sm :add-transition + :move-elevator-front :set-param-for-elevator-inside t) + (send sm :add-transition :move-elevator-front :fail nil) + (send sm :add-transition + :set-param-for-elevator-inside :check-elevator-open t) + (send sm :add-transition :check-elevator-open :speak-to-human-behind t) + (send sm :add-transition :check-elevator-open :fail nil) + (send sm :add-transition :speak-to-human-behind :move-elevator-inside t) + (send sm :add-transition :move-elevator-inside :succeed t) + (send sm :add-transition :move-elevator-inside :fail nil) + sm)) + +(defun take-elevator-state-machine () + (let ((sm (instance state-machine :init)) + (move-client (pr2-interface-move-base-action *ri*))) + (send sm :add-node + (instance state :init :move-front + '(lambda (userdata) + (send *ri* :move-to + (cdr (assoc 'front-coords userdata)) + :retry 10)))) + (send sm :add-node (instance state :init :push-outside (push-state-machine) + :remap-list '((panel-name . outside-panel-name) + (button . outside-button)))) + (send sm :add-node + (instance state :init :speak-fail + '(lambda (x) + (ros::ros-warn "Could not take elevator.") + (speak-jp "エレベータに乗れませんでした。" :wait t) + t))) + (send sm :add-node + (instance state :init :move-inside (move-inside-state-machine) + :remap-list '((elevator-front-coords . elevator-front-coords) + (inside-coords . inside-coords)))) + (send sm :add-node (instance state :init :push-inside (push-state-machine) + :remap-list '((panel-name . inside-panel-name) + (button . target-floor)))) + (send sm :add-node + (instance state :init :ready-to-outside 'ready-to-outside)) + (send sm :add-node (instance state :init :move-outside 'move-outside)) + (send sm :add-node + (instance state :init :prepare-normal-movement + '(lambda (userdata) + ;; Free arm may hold something, + ;; so keep it away from base laser + (pr2-tuckarm-pose + (case (cdr (assoc 'push-arm userdata)) + (:rarm :larm) + (:larm :rarm))) + (call-service "look_forward_in_navigation/start" + (instance std_srvs::EmptyRequest :init)) + (restore-params)))) + (send sm :arg-keys 'elevator-front-coords 'front-coords 'outside-panel-name + 'outside-button 'inside-coords 'inside-panel-name + 'target-floor 'outside-coords 'push-arm) + (send sm :goal-state (list :succeed :fail-outside)) + (send sm :start-state :move-front) + (send sm :add-transition :move-front :push-outside t) + (send sm :add-transition :move-front :fail-outside nil) + (send sm :add-transition :push-outside :move-inside :succeed) + (send sm :add-transition :move-inside :push-inside :succeed) + (send sm :add-transition :move-inside :speak-fail :fail) + (send sm :add-transition :speak-fail :move-front t) + (send sm :add-transition :push-inside :ready-to-outside :succeed) + (send sm :add-transition :ready-to-outside :move-outside :succeed) + (send sm :add-transition :ready-to-outside :push-inside :fail) + (send sm :add-transition :move-outside :prepare-normal-movement t) + (send sm :add-transition :move-outside :push-inside nil) + (send sm :add-transition :prepare-normal-movement :succeed t) + sm)) + +(defun elevator-move-base-state-machine () + (let ((sm (instance state-machine :init))) + (send sm :add-node + (instance state :init :check-elevator-needed 'check-elevator-needed)) + (send sm :add-node (instance state :init :sanity-check 'sanity-check)) + (send sm :add-node + (instance state :init :speak-elevator + '(lambda (userdata) + ;; Free arm may hold something, + ;; so keep it away from base laser + (pr2-tuckarm-pose + (case (cdr (assoc 'push-arm userdata)) + (:rarm :larm) + (:larm :rarm))) + (store-params) + (call-service "look_forward_in_navigation/stop" + (instance std_srvs::EmptyRequest :init)) + (tolerance-medium) + (use-tilt-laser-obstacle-cloud t) + (ros::ros-info "Go to elevator.") + (speak-jp "エレベータに向かいます。" :wait t) + t))) + (send sm :add-node + (instance state :init :take-elevator (take-elevator-state-machine))) + (send sm :add-node + (instance state :init :go-to + '(lambda (userdata) + (use-tilt-laser-obstacle-cloud t) + (send *ri* :move-to + (cdr (assoc 'target-coords userdata)) + :retry 10 :correction nil)))) + (send sm :arg-keys 'elevator-front-coords 'front-coords 'outside-panel-name + 'outside-button 'inside-coords 'inside-panel-name + 'target-floor 'outside-coords 'target-coords + 'push-arm :goal) + (send sm :goal-state (list :succeed :fail)) + (send sm :start-state :check-elevator-needed) + (send sm :add-transition :check-elevator-needed :sanity-check t) + (send sm :add-transition :check-elevator-needed :go-to nil) + (send sm :add-transition :sanity-check :speak-elevator t) + (send sm :add-transition :sanity-check :fail nil) + (send sm :add-transition :speak-elevator :take-elevator t) + (send sm :add-transition :take-elevator :go-to :succeed) + (send sm :add-transition :take-elevator :fail :fail-outside) + (send sm :add-transition :go-to :succeed t) + (send sm :add-transition :go-to :fail nil) + sm)) diff --git a/elevator_move_base_pr2/src/state/check-elevator-needed.l b/elevator_move_base_pr2/src/state/check-elevator-needed.l new file mode 100644 index 0000000000..79af3e234e --- /dev/null +++ b/elevator_move_base_pr2/src/state/check-elevator-needed.l @@ -0,0 +1,79 @@ +(load "package://elevator_move_base_pr2/src/utils.l") + + +(defvar *floors*) +(defvar *scene*) +(defvar *tfl*) + + +(defun check-elevator-needed (userdata) + (ros::ros-info "Checking if elevator is needed...") + (update-robot-position) + (let* ((goal (cdr (assoc :goal userdata))) + (goal-pose (ros::tf-pose-stamped->coords + (send goal :target_pose))) + (cur-floor (check-current-floor *scene* *tfl*)) + (target-floor (check-current-floor *scene* *tfl* goal-pose)) + target-floor-button + (target-coords + (transform-pose-to-target-frame goal-pose (send *scene* :name) *tfl*)) + up/down) + (ros::ros-info + (format nil + "cur-floor: ~A, target-floor: ~A, target-coords: ~A" + cur-floor target-floor target-coords)) + (setq up/down (cond ((send *scene* :floor< target-floor cur-floor) "down") + ((send *scene* :floor< cur-floor target-floor) "up") + (t nil))) + (ros::ros-info (format nil "up/down: ~A" up/down)) + (setq target-floor-button + (elt (assoc target-floor *floors* :test #'string= + :key #'(lambda (x) + (format nil "~A/~A" (send *scene* :name) + (string-downcase (elt x 0))))) + 0)) ;; /eng8/1f -> 1F + (set-alist 'inside-panel-name "/elevator_inside_panel" userdata) + (set-alist 'outside-panel-name "/elevator_call_panel" userdata) + ;; transform to scene frame + (set-alist + 'elevator-front-coords + (let ((coords (send *scene* :transformation + (car (send *scene* :find-object + (format nil "~A/elevator-outside" + cur-floor)))))) + (send coords :name (send *scene* :name)) + (send coords :rotate pi :z) + (send coords :translate #f(0 -200 0) :world) + coords) + userdata) + (set-alist + 'front-coords + (let ((coords (send *scene* :transformation + (car (send *scene* :find-object + (format nil "~A/elevator_call_panel-front" + cur-floor)))))) + (send coords :name (send *scene* :name)) + coords) + userdata) + (set-alist + 'inside-coords + (let ((coords (send *scene* :transformation + (car (send *scene* :find-object + (format nil "~A/elevator_inside_panel-front" + cur-floor)))))) + (send coords :name (send *scene* :name)) + coords) + userdata) + (set-alist + 'outside-coords + (let ((coords (send *scene* :transformation + (car (send *scene* :find-object + (format nil "~A/elevator-outside" + target-floor)))))) + (send coords :name (send *scene* :name)) + coords) + userdata) + (set-alist 'target-floor target-floor-button userdata) + (set-alist 'outside-button up/down userdata) + (set-alist 'target-coords target-coords userdata) + (not (null up/down)))) diff --git a/elevator_move_base_pr2/src/state/check-elevator-open.l b/elevator_move_base_pr2/src/state/check-elevator-open.l new file mode 100644 index 0000000000..2c65ab12c3 --- /dev/null +++ b/elevator_move_base_pr2/src/state/check-elevator-open.l @@ -0,0 +1,47 @@ +(ros::roseus-add-msgs "sensor_msgs") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") + + +;; (defvar *pr2*) +;; (defvar *ri*) + + +(defun check-elevator-open (&key (timeout 300)) + (let ((topic + "/check_elevator_open/octree_change_publisher/octree_change_result") + (change-thre (ros::get-param "change_threshold" 400)) + (change-pc nil) time-start) + (send *pr2* :head :angle-vector #f(0 0)) + (send *ri* :angle-vector (send *pr2* :angle-vector) 400) + (send *ri* :wait-interpolation) + + (setq time-start (ros::time-now)) + (ros::subscribe + topic sensor_msgs::PointCloud2 + #'(lambda (msg) + (when msg + (let ((stamp (send msg :header :stamp))) + ;; Wait until point cloud becomes stable + (when (ros::time> stamp (ros::time+ time-start (ros::time 0.5))) + (setq change-pc (* (send msg :height) (send msg :width))) + (ros::ros-info "pointcloud change: ~A, open/closed threshold: ~A" + change-pc change-thre)))))) + + (ros::rate 10) + (while (or (null change-pc) (< change-pc change-thre)) + (ros::spin-once) + (when (ros::time> (ros::time-now) + (ros::time+ time-start (ros::time timeout))) + (ros::warn + (format nil "Could not detect door opening in ~A [sec]." timeout)) + (speak-jp "ドアが開いていません。") + (ros::unsubscribe topic) + (return-from check-elevator-open nil)) + (ros::sleep)) + + (ros::unsubscribe topic) + (ros::ros-info "Door opened.") + (speak-jp "ドアが開きました。") + (clear-costmap) + t)) diff --git a/elevator_move_base_pr2/src/state/move-outside.l b/elevator_move_base_pr2/src/state/move-outside.l new file mode 100644 index 0000000000..ad6a5013bb --- /dev/null +++ b/elevator_move_base_pr2/src/state/move-outside.l @@ -0,0 +1,26 @@ +(require :pr2-interface "package://pr2eus/pr2-interface.l") + +(load "package://elevator_move_base_pr2/src/utils.l") + + +(defvar *scene*) +(defvar *tfl*) + + +(defun move-outside (userdata) + (unix:sleep 3) ;; Wait for door opening + (speak-jp "エレベータからおります。") + (clear-costmap) + (if (send *ri* :move-to (cdr (assoc 'outside-coords userdata)) :retry 2) + t + (progn + (speak-jp "エレベータから降りられませんでした。") + ;; Return to elevator_inside_panel-front + (setq panel-front-coords + (send *scene* :transformation + (car (send *scene* :find-object + (format nil "~A/elevator_inside_panel-front" + (check-current-floor *scene* *tfl*)))))) + (send panel-front-coords :name (send *scene* :name)) + (send *ri* :move-to panel-front-coords :retry 2) + nil))) diff --git a/elevator_move_base_pr2/src/state/push-elevator-button.l b/elevator_move_base_pr2/src/state/push-elevator-button.l new file mode 100644 index 0000000000..02f5a0712d --- /dev/null +++ b/elevator_move_base_pr2/src/state/push-elevator-button.l @@ -0,0 +1,259 @@ +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "move_base_msgs") +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-srvs "std_srvs") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") +(require :state-machine "package://roseus_smach/src/state-machine.l") + +(load "package://elevator_move_base_pr2/src/utils.l") + + +(defvar *panel*) +;; (defvar *pr2*) +;; (defvar *ri*) +(defvar *scene*) +(defvar *tfl*) + + +(defclass light-button-cb-class + :super propertied-object + :slots (value timestamp tau)) + +(defmethod light-button-cb-class + (:init () + (setq value 0) + (setq timestamp (ros::time 0)) + (setq tau 1.0) ;; tau = half-life + (ros::subscribe "light_button" std_msgs::float32 #'send self :cb)) + + (:cb (msg) + (let* ((diff (send (ros::time- (ros::time-now) timestamp) :to-sec)) + (weight (exp (* (log 0.5) (/ diff tau))))) + (setq timestamp (ros::time-now)) + (setq value (+ (* weight value) (* (- 1 weight) (send msg :data)))))) + + ;; if the last observation is too old, the score is 0 + (:value () + (if (< tau (send (ros::time- (ros::time-now) timestamp) :to-sec)) + (setq value 0) + value))) + + +(defun update (&optional (times 10)) + (let (updated-object (count 0) (found 0) (panel-moved nil)) + ;; wait for result + (ros::rate 10) + + (while (<= (incf count) times) + (ros::spin-once) + (update-robot-position) + (setq updated-object (update-scene-by-tf *scene* *tfl*)) + (when (memq *panel* updated-object) + (setq panel-moved t) + (incf found)) + (ros::ros-info + (format nil "count: ~A, found: ~A, panel position: ~A" + count found (send *panel* :worldpos))) + (when (>= found 3) + (return)) + (ros::sleep)) + panel-moved)) + +(defun set-view-target (coords camera frame-id) + (let ((msg (instance geometry_msgs::PointStamped :init)) + (uv (send camera :screen-point (send coords :worldpos)))) + ;; out of view + (unless (and (<= 0 (elt uv 0) (send camera :width)) + (<= 0 (elt uv 1) (send camera :height))) + (ros::ros-warn "screen-point: ~A is out of view (~A, ~A)" + uv (send camera :width) (send camera :height)) + (return-from set-view-target nil)) + (send msg :header :stamp (ros::time-now)) + (send msg :header :frame_id frame-id) + (send msg :point :x (elt uv 0)) + (send msg :point :y (elt uv 1)) + (send msg :point :z 10) ;; radius [px] + (ros::publish "view_target" msg) + t)) + +(defun push-button (target-coords arm) + (let (rayvec via-coords + via-angle-vector target-angle-vector org-arm-anglev org-head-anglev + reset-pose + (rarm-reset #f(-60 70 -70 -120 160 -30 180)) ;; 2nd element 80 -> 70 + (larm-reset #f(60 70 70 -120 -160 -30 180))) ;; 2nd element 80 -> 70 + (ros::spin-once) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector)) + ;; if the free-arm is grasping something // TODO: smart arm change motion + (ros::ros-info (format nil "start-grasp -> ~A" (send *ri* :start-grasp :arms))) + + (setq rayvec + (normalize-vector + (v- (float-vector + (elt (send *pr2* :laser_tilt_mount_link_lk :worldpos) 0) + (elt (send *pr2* :laser_tilt_mount_link_lk :worldpos) 1) + (elt (send target-coords :worldpos) 2)) + (send target-coords :worldpos)))) + + ;; via-coords + (setq via-coords (send (send target-coords :copy-worldcoords) + :translate (scale 50 rayvec) :world)) + ;; push 15mm + (setq target-coords (send (send target-coords :copy-worldcoords) + :translate (scale -15 rayvec) :world)) + + ;; Select which arm to use + (unless arm + (setq arm + (if (plusp (elt (send (send *pr2* :transformation target-coords) + :worldpos) 1)) + :larm + :rarm))) + (ros::ros-info (format nil "Select ~A arm to push button ~A" + arm (send target-coords :worldpos))) + + (setq org-head-anglev (send *pr2* :head :angle-vector)) + + ;; reset-pose (arms only) and select arm + (send *pr2* :reset-pose) + (send *pr2* :head :angle-vector org-head-anglev) + (setq reset-pose (send *pr2* :angle-vector)) + (unless (eps-v= reset-pose (send *ri* :state :potentio-vector) 2.0) + (send *ri* :angle-vector reset-pose 3000) + (send *ri* :wait-interpolation)) + + (setq org-arm-anglev (send *pr2* arm :angle-vector)) + + ;; Check if IK is solvable + (send *pr2* arm :angle-vector (case arm (:rarm rarm-reset) + (:larm larm-reset))) + (unless (send *pr2* arm :inverse-kinematics via-coords :rotation-axis :x) + (ros::ros-warn + (format nil "Could not solve IK to via-coords: ~A" via-coords)) + (send *pr2* arm :angle-vector org-arm-anglev) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (return-from push-button nil)) + (setq via-angle-vector (send *pr2* :angle-vector)) + + (unless (send *pr2* arm :inverse-kinematics target-coords :rotation-axis :x) + (ros::ros-warn + (format nil "Could not solve IK to target-coords: ~A" target-coords)) + (send *pr2* arm :angle-vector org-arm-anglev) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + (send *ri* :wait-interpolation) + (return-from push-button nil)) + (setq target-angle-vector (send *pr2* :angle-vector)) + + ;; Push + (send *ri* :angle-vector via-angle-vector 1500) + (send *ri* :wait-interpolation) + (send *ri* :angle-vector target-angle-vector 600) + (send *ri* :wait-interpolation) + (send *ri* :angle-vector via-angle-vector 400) + (send *ri* :wait-interpolation) + + ;; Return to original pose, but no :wait-interpolation + (send *pr2* arm :angle-vector org-arm-anglev) + (send *ri* :angle-vector (send *pr2* :angle-vector) 1000) + t)) + +(defun look-button-state (userdata) + (let ((panel-name (cdr (assoc 'panel-name userdata))) look-target look-av) + (update-robot-position) + + (ros::ros-info "panel-name: ~A" panel-name) + (setq *panel* (car (send *scene* :find-object panel-name))) + + (setq look-target + (car (send *scene* :find-object + (format nil "~A~A" (check-current-floor *scene* *tfl*) + panel-name)))) + (ros::ros-info "Look at panel position: ~A" (send look-target :worldpos)) + (send *pr2* :reset-pose) + (send *pr2* :head :look-at (send look-target :worldpos)) + (setq look-av (send *pr2* :angle-vector)) + + ;; Move only head quickly + (send *ri* :spin-once) + (unless (eps-v= (float-vector (elt look-av 15) (elt look-av 16)) + (float-vector (elt (send *ri* :state :potentio-vector) 15) + (elt (send *ri* :state :potentio-vector) 16)) + 1.0) + (send *ri* :angle-vector look-av 1000 :head-controller) + (send *ri* :wait-interpolation)) + + ;; Move arms and torso only when it is needed (i.e. skipped inside elevator) + (if (equal panel-name "/elevator_call_panel") + (send *ri* :angle-vector look-av 5000) + (send *ri* :wait-interpolation)) + + (if (equal panel-name "/elevator_call_panel") + (call-service + "/narrow_stereo/left/pass_through_feature_to_call_panel/request" + (instance std_srvs::EmptyRequest :init)) + (call-service + "/narrow_stereo/left/pass_through_feature_to_inside_panel/request" + (instance std_srvs::EmptyRequest :init))) + + ;; Wait until latest image is available. + (unix:usleep (* 500 1000)) + + ;; Update *scene* (especially pose of *panel*) by TF + (ros::ros-info "Looking for button...") + (speak-jp "ボタンを探しています。") + (ros::ros-info "Waiting for vision update...") + (unless (update 10) + (ros::ros-info "Could not find button.") + (speak-jp "見つかりませんでした。") + (return-from look-button-state nil)) + (if (equal panel-name "/elevator_call_panel") + (call-service + "/narrow_stereo/left/pass_through_feature_to_call_panel/stop" + (instance std_srvs::EmptyRequest :init)) + (call-service + "/narrow_stereo/left/pass_through_feature_to_inside_panel/stop" + (instance std_srvs::EmptyRequest :init))) + t)) + +(defun push-button-state (userdata) + (let* ((button (cdr (assoc 'button userdata))) + (button-coords (find-if #'(lambda (o) (string= button (send o :name))) + (flatten (send *panel* :inheritance)))) + (push-arm (cdr (assoc 'push-arm userdata)))) + (ros::ros-info (format nil "push button (~a) -> ~a" button button-coords)) + (speak-jp "ボタンを押します。") + (push-button button-coords push-arm))) + +(defun check-button-light-state (userdata) + (let* ((button (cdr (assoc 'button userdata))) + (button-coords (find-if #'(lambda (o) (string= button (send o :name))) + (flatten (send *panel* :inheritance)))) + (button-state (instance light-button-cb-class :init)) + (start-tm (ros::time-now)) + (timeout 2.0)) + (call-service "/wide_stereo/left/pass_through_image_rect_color/request" + (instance std_srvs::EmptyRequest :init)) + (set-view-target button-coords (send *pr2* :wide_stereo-left) + "wide_stereo_optical_frame") + ;; check phase + (ros::rate 10) + (while (and (ros::ok) + (ros::time< (ros::time-now) + (ros::time+ start-tm (ros::time timeout)))) + (ros::spin-once) + (ros::ros-info "button-state: ~A" (send button-state :value)) + (when (< 0.5 (send button-state :value)) + (ros::ros-info "Button is lighting up.") + (speak-jp "ボタンが光りました。") + (call-service "/wide_stereo/left/pass_through_image_rect_color/stop" + (instance std_srvs::EmptyRequest :init)) + (clear-costmap) + (return-from check-button-light-state t)) + (ros::sleep)) + (ros::ros-info "Button is not lighting up.") + (speak-jp "押せていないようです。") + (call-service "/wide_stereo/left/pass_through_image_rect_color/stop" + (instance std_srvs::EmptyRequest :init)) + nil)) diff --git a/elevator_move_base_pr2/src/state/ready-to-outside.l b/elevator_move_base_pr2/src/state/ready-to-outside.l new file mode 100644 index 0000000000..027c6c44e8 --- /dev/null +++ b/elevator_move_base_pr2/src/state/ready-to-outside.l @@ -0,0 +1,136 @@ +(ros::roseus-add-msgs "geometry_msgs") +(ros::roseus-add-msgs "roseus") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") + +(load "package://elevator_move_base_pr2/src/ros-callback-manager.l") +(load "package://elevator_move_base_pr2/src/state/check-elevator-open.l") +(load "package://elevator_move_base_pr2/src/state/push-elevator-button.l") +(load "package://elevator_move_base_pr2/src/utils.l") + + +(defvar *floors*) +(defvar *panel*) +;; (defvar *pr2*) +;; (defvar *ri*) +(defvar *scene*) +(defvar *tfl*) + + +(defun check-target-floor-arrived (target-floor &key (timeout 5)) + (let ((topic + (instance ros-callback-message + :init roseus::StringStamped "/elevator_number/result")) + (prev-reset-time (ros::time-now)) + (prev-floor nil) + (cur-floor nil) + button-coords + (button-state (instance light-button-cb-class :init))) + (call-service + "/narrow_stereo/left/pass_through_feature_to_inside_panel/request" + (instance std_srvs::EmptyRequest :init)) + (call-service "/wide_stereo/left/pass_through_image_rect_color/request" + (instance std_srvs::EmptyRequest :init)) + (ros::rate 10) + (while (ros::ok) + (update 1) + (setq button-coords + (find-if #'(lambda (o) (string= target-floor (send o :name))) + (flatten (send *panel* :inheritance)))) + (set-view-target button-coords (send *pr2* :wide_stereo-left) + "wide_stereo_optical_frame") + (ros::spin-once) + (when (send topic :msg) + (setq cur-floor (string-upcase (send topic :msg :data)))) + (unless (equal cur-floor prev-floor) ;; Elevator moved, so reset time. + (ros::ros-info (format nil "Current floor: ~A" cur-floor)) + (setq prev-reset-time (ros::time-now)) + (setq prev-floor cur-floor)) + (when (and (ros::time> (ros::time-now) + (ros::time+ prev-reset-time (ros::time timeout))) + (< (send button-state :value) 0.5)) + (ros::warn + (format nil "Elevator has not moved in recent ~A [sec]." timeout)) + (speak-jp "エレベータが動いていません。") + (call-service + "/narrow_stereo/left/pass_through_feature_to_inside_panel/stop" + (instance std_srvs::EmptyRequest :init)) + (call-service "/narrow_stereo/left/pass_through_image_rect/stop" + (instance std_srvs::EmptyRequest :init)) + (call-service "/wide_stereo/left/pass_through_image_rect_color/stop" + (instance std_srvs::EmptyRequest :init)) + (return-from check-target-floor-arrived nil)) + (when (and cur-floor + (substringp (concatenate string "/" target-floor) cur-floor)) + (call-service + "/narrow_stereo/left/pass_through_feature_to_inside_panel/stop" + (instance std_srvs::EmptyRequest :init)) + (call-service "/narrow_stereo/left/pass_through_image_rect/stop" + (instance std_srvs::EmptyRequest :init)) + (call-service "/wide_stereo/left/pass_through_image_rect_color/stop" + (instance std_srvs::EmptyRequest :init)) + (return-from check-target-floor-arrived t)) + (ros::sleep) + ))) + +(defun change-floor (target-floor scene tfl + &optional (topicname "/initialpose3d")) + (ros::advertise topicname geometry_msgs::PoseWithCovarianceStamped 1) + (unix:usleep (* 200 1000)) ;; Wait for publisher to be ready + (let* ((msg (instance geometry_msgs::PoseWithCovarianceStamped :init)) + (current-floor-frame (check-current-floor scene tfl)) + (robot-pose (send tfl :lookup-transform + current-floor-frame "base_footprint" (ros::time 0)))) + (unless robot-pose + (return-from change-floor nil)) + (send msg :header :frame_id target-floor) + (send msg :header :stamp (ros::time-now)) + (let ((cov (send msg :pose :covariance))) + (setf (elt cov 0) 0.05) + (setf (elt cov 7) 0.05) + (setf (elt cov 21) 0.02)) + (send msg :pose :pose (ros::coords->tf-pose robot-pose)) + (ros::publish topicname msg))) + +(defun ready-to-outside (userdata) + (call-service + "/narrow_stereo/left/pass_through_feature_to_inside_panel/request" + (instance std_srvs::EmptyRequest :init)) + (call-service "/narrow_stereo/left/pass_through_image_rect/request" + (instance std_srvs::EmptyRequest :init)) + (clear-costmap) + (tolerance-loose) + (send *ri* :go-pos-unsafe 0 0 90) ;; Unsafe for faster turning left + + (update-robot-position) + ;; Look at panel + (let ((look-target + (or (find-if #'(lambda (x) (string= (send x :name) "/elevator_number")) + (send *panel* :descendants)) + *panel*))) + (send *pr2* :head :look-at (send look-target :worldpos)) + ;; Move fast but converge to the target angle vector + (until (eps-v= (send *pr2* :angle-vector) + (send *ri* :state :potentio-vector) + 1.0) + (send *ri* :angle-vector (send *pr2* :angle-vector) 500) + (send *ri* :wait-interpolation))) + + (let* ((target-floor (cdr (assoc 'target-floor userdata)))) + (unless (check-target-floor-arrived target-floor :timeout 5) + (send *ri* :go-pos-unsafe 0 0 -90) ;; Unsafe for faster turning right + (return-from ready-to-outside :fail)) + + (dolist (floor *floors*) + (when (equal target-floor (elt floor 0)) + (ros::ros-info (format nil "Arrived at ~A." (elt floor 0))) + (speak-jp (concatenate string (elt floor 1) "に着きました。")) + (change-floor (format nil "~A/~a" (send *scene* :name) + (string-downcase (elt floor 0))) + *scene* *tfl*)))) + + (unless (check-elevator-open :timeout 6) + (send *ri* :go-pos-unsafe 0 0 -90) ;; Unsafe for faster turning right + (return-from ready-to-outside :fail)) + (unix:usleep (* 500 1000)) ;; Wait until door is open + :succeed) diff --git a/elevator_move_base_pr2/src/state/sanity-check.l b/elevator_move_base_pr2/src/state/sanity-check.l new file mode 100644 index 0000000000..d8fc398c87 --- /dev/null +++ b/elevator_move_base_pr2/src/state/sanity-check.l @@ -0,0 +1,85 @@ +(ros::roseus-add-msgs "jsk_recognition_msgs") +(ros::roseus-add-msgs "sensor_msgs") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") + +(load "package://elevator_move_base_pr2/src/utils.l") + + +;; (defvar *ri*) + + +(defun speak-subscription-error (topic) + (ros::ros-error (format nil "Could not subscribe ~A." topic)) + ;; e.g. "/narrow_stereo/left/image_rect" -> "narrow stereo left image rect" + (speak-jp + (format nil "~Aのトピックをsubscribeできませんでした。" + (string-trim + " " (substitute #\Space #\_ (substitute #\Space #\/ topic)))) + :wait t)) + +(defun check-topic-published (topic type &optional (timeout 10)) + (ros::ros-info (format nil "Start subscribing ~A..." topic)) + (when (one-shot-subscribe + topic type :timeout (* timeout 1000) :after-stamp (ros::time-now)) + (return-from check-topic-published t)) + (speak-subscription-error topic) + nil) + +(defun sanity-check (userdata) + (let ((success t)) + (ros::ros-info "Start sanity-check...") + (unless (boundp '*ri*) + (pr2-init)) + + ;; Sensors + (unless (check-topic-published + "/narrow_stereo/left/image_rect" sensor_msgs::Image 10) + (setq success nil)) + (unless (check-topic-published + "/wide_stereo/left/image_rect_color" sensor_msgs::Image 10) + (setq success nil)) + (unless (check-topic-published + "/kinect_head/depth_registered/points" sensor_msgs::PointCloud2 10) + (setq success nil)) + (unless (check-topic-published "/base_scan" sensor_msgs::LaserScan 2) + (setq success nil)) + (unless (check-topic-published "/tilt_scan" sensor_msgs::LaserScan 2) + (setq success nil)) + + ;; For look-at-human + (unless (check-topic-published "edgetpu_human_pose_estimator/output/poses" + jsk_recognition_msgs::PeoplePoseArray 5) + (setq success nil)) + + ;; For door button recognition with FCN + (call-service "door_button/pass_through_image/request" + (instance std_srvs::EmptyRequest :init)) + (unless (check-topic-published + "door_button/fcn_object_segmentation/output" sensor_msgs::Image 20) + (setq success nil)) + (call-service "door_button/pass_through_image/stop" + (instance std_srvs::EmptyRequest :init)) + + ;; For human in mirror recognition with FCN + (call-service "human_in_mirror/pass_through_kinect_rgb/request" + (instance std_srvs::EmptyRequest :init)) + (call-service "human_in_mirror/pass_through_kinect_depth/request" + (instance std_srvs::EmptyRequest :init)) + (unless (check-topic-published + "human_in_mirror/find_human_in_mirror/output/inside_mirror" + jsk_recognition_msgs::BoolStamped 20) + (setq success nil)) + (unless (check-topic-published + "human_in_mirror/find_human_in_mirror/output/outside_mirror" + jsk_recognition_msgs::BoolStamped 20) + (setq success nil)) + (call-service "human_in_mirror/pass_through_kinect_rgb/stop" + (instance std_srvs::EmptyRequest :init)) + (call-service "human_in_mirror/pass_through_kinect_depth/stop" + (instance std_srvs::EmptyRequest :init)) + + (if success + (ros::ros-info "Successfully finished sanity-check.") + (ros::ros-error "Failed to execute sanity-check.")) + success)) diff --git a/elevator_move_base_pr2/src/state/speak-to-human-behind.l b/elevator_move_base_pr2/src/state/speak-to-human-behind.l new file mode 100644 index 0000000000..1a405e32dc --- /dev/null +++ b/elevator_move_base_pr2/src/state/speak-to-human-behind.l @@ -0,0 +1,64 @@ +(ros::roseus-add-msgs "jsk_recognition_msgs") +(ros::roseus-add-srvs "std_srvs") + +(require :pr2-interface "package://pr2eus/pr2-interface.l") + +(load "package://elevator_move_base_pr2/src/utils.l") + + +(defun speak-to-human-behind (userdata) + (let (start-tm (timeout (ros::time 2.5)) + (sub-inside-num 0) (sub-outside-num 0) (sub-req 2) + (inside-p nil) (outside-p nil)) + (ros::subscribe + "/human_in_mirror/find_human_in_mirror/output/inside_mirror" + jsk_recognition_msgs::BoolStamped + #'(lambda (m) + (when (ros::time< start-tm (send m :header :stamp)) + (incf sub-inside-num) + (setq inside-p (send m :data))))) ;; Use only latest data + (ros::subscribe + "/human_in_mirror/find_human_in_mirror/output/outside_mirror" + jsk_recognition_msgs::BoolStamped + #'(lambda (m) + (when (ros::time< start-tm (send m :header :stamp)) + (incf sub-outside-num) + (setq outside-p (send m :data))))) ;; Use only latest data + (unix:usleep (* 2500 1000)) ;; Wait for door opening + (call-service "/human_in_mirror/pass_through_kinect_rgb/request" + (instance std_srvs::EmptyRequest :init)) + (call-service "/human_in_mirror/pass_through_kinect_depth/request" + (instance std_srvs::EmptyRequest :init)) + + (setq start-tm (ros::time-now)) + (while (and (ros::ok) + (or (< sub-inside-num sub-req) (< sub-outside-num sub-req)) + (ros::time< (ros::time-now) (ros::time+ start-tm timeout))) + (ros::spin-once) + (ros::sleep)) + + ;; Speak fast! + (cond ((and inside-p outside-p) + (ros::ros-info + "Speaking to human behind robot and inside elevator...") + (speak-jp "後ろの方お先に失礼します、中の方お邪魔します。" + :wait nil)) + ((and inside-p (not outside-p)) + (ros::ros-info "Speaking to human behind robot...") + (speak-jp "後ろの方お先に失礼します。" :wait nil)) + ((and (not inside-p) outside-p) + (ros::ros-info "Speaking to human inside elevator...") + (speak-jp "エレベータ内の方お邪魔します。" :wait nil)) + (t + (ros::ros-info "Nobody was found. Move into elevator.") + (speak-jp "エレベータに入ります。" :wait nil))) + (call-service "/human_in_mirror/pass_through_kinect_rgb/stop" + (instance std_srvs::EmptyRequest :init)) + (call-service "/human_in_mirror/pass_through_kinect_depth/stop" + (instance std_srvs::EmptyRequest :init)) + (ros::unsubscribe + "/human_in_mirror/find_human_in_mirror/output/inside_mirror") + (ros::unsubscribe + "/human_in_mirror/find_human_in_mirror/output/outside_mirror") + (clear-costmap) + t)) diff --git a/elevator_move_base_pr2/src/utils.l b/elevator_move_base_pr2/src/utils.l new file mode 100644 index 0000000000..79291d8474 --- /dev/null +++ b/elevator_move_base_pr2/src/utils.l @@ -0,0 +1,145 @@ +(ros::roseus-add-msgs "move_base_msgs") +(ros::roseus-add-msgs "std_msgs") +(ros::roseus-add-srvs "std_srvs") + +(require :eng2-scene "package://jsk_maps/src/eng2-scene.l") +(require :eng8-scene "package://jsk_maps/src/eng8-scene.l") +(require :pr2-interface "package://pr2eus/pr2-interface.l") + + +;; (defvar *pr2*) +;; (defvar *ri*) + + +(defun call-service (service req) + (if (ros::wait-for-service service 0) + (ros::service-call service req) + nil)) + +(defun make-scene-by-tf (tfl) + "Load building scene by existing TF frame" + (let (frames scene) + (unix::sleep 1) ;; wait for TF messages + + (setq frames (send tfl :get-frame-strings)) + (cond ((find "eng2" frames :test #'string=) + (setq scene (make-eng2-scene))) + ((find "eng8" frames :test #'string=) + (setq scene (make-eng8-scene))) + (t + (ros::ros-fatal + "There is no building frame_id in TF tree, so exiting...") + (exit 1)) + ) + scene)) + +(defun update-scene-by-tf (scene tfl &optional (tm (ros::time 0)) (timeout 0.1)) + (let* ((objects (send scene :objects)) + (fixed-frame (send scene :name)) ;; ok? + (updated nil) + trans) + (dolist (obj objects) + (when (and (stringp (send obj :name)) + (send tfl :wait-for-transform fixed-frame (send obj :name) + tm timeout)) + (setq trans + (send tfl :lookup-transform fixed-frame (send obj :name) tm)) + (send obj :move-to trans scene) + (push obj updated) + )) + updated)) + +(defun update-robot-position () + (ros::spin-once) + (send *pr2* :move-to (send *ri* :state :worldcoords) :world) + (send *pr2* :angle-vector (send *ri* :state :potentio-vector))) + +(defun coords->movebaseactiongoal (coords) + (let ((goal (instance move_base_msgs::movebaseactiongoal :init))) + (send goal :goal :target_pose :pose (ros::coords->tf-pose coords)) + (send goal :goal :target_pose :header :frame_id (send coords :name)) + goal)) + +(defun transform-pose-to-target-frame (coords frame-id tfl + &optional (tm (ros::time-now)) + (timeout 1.0)) + (send tfl :wait-for-transform frame-id (send coords :name) tm timeout) + (let ((tra (send tfl :lookup-transform frame-id (send coords :name) tm))) + (when tra + (send tra :transform coords) + (send tra :name frame-id) + tra))) + +(defun check-current-floor (scene tfl &optional pose (tm (ros::time-now))) + ;; :name == frame_id + (if pose + (setq pose (transform-pose-to-target-frame pose (send scene :name) tfl)) + (progn + (send tfl :wait-for-transform (send scene :name) "base_footprint" tm 1.0) + (setq pose (send tfl :lookup-transform + (send scene :name) "base_footprint" tm)))) + (send scene :current-floor pose)) + +(defun tolerance-loose (&rest args) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "xy_goal_tolerance" :double 0.20) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "yaw_goal_tolerance" :double 0.175) ;; 10deg + t) + +(defun tolerance-medium (&rest args) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "xy_goal_tolerance" :double 0.10) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "yaw_goal_tolerance" :double 0.087) ;; 5deg + (ros::set-dynamic-reconfigure-param + "/move_base_node/global_costmap" "footprint_padding" :double 0.05) + (ros::set-dynamic-reconfigure-param + "/move_base_node/local_costmap" "footprint_padding" :double 0.05) + t) + +(defun tolerance-strict (&rest args) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "xy_goal_tolerance" :double 0.10) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "yaw_goal_tolerance" :double 0.087) ;; 5deg + (ros::set-dynamic-reconfigure-param + "/move_base_node/global_costmap" "footprint_padding" :double 0.03) + (ros::set-dynamic-reconfigure-param + "/move_base_node/local_costmap" "footprint_padding" :double 0.03) + t) + +(defun store-params (&rest args) + (defparameter *xy-goal-tolerance* + (ros::get-param "/move_base_node/DWAPlannerROS/xy_goal_tolerance")) + (defparameter *yaw-goal-tolerance* + (ros::get-param "/move_base_node/DWAPlannerROS/yaw_goal_tolerance")) + (defparameter *global-footprint-padding* + (ros::get-param "/move_base_node/global_costmap/footprint_padding")) + (defparameter *local-footprint-padding* + (ros::get-param "/move_base_node/local_costmap/footprint_padding")) + (defparameter *look-at-human-enabled* + (send (one-shot-subscribe "look_at_human/enabled" std_msgs::Bool) :data)) + (call-service "look_at_human/stop" (instance std_srvs::EmptyRequest :init)) + t) + +(defun restore-params (&rest args) + (when (boundp '*xy-goal-tolerance*) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "xy_goal_tolerance" + :double *xy-goal-tolerance*)) + (when (boundp '*yaw-goal-tolerance*) + (ros::set-dynamic-reconfigure-param + "/move_base_node/DWAPlannerROS" "yaw_goal_tolerance" + :double *yaw-goal-tolerance*)) + (when (boundp '*global-footprint-padding*) + (ros::set-dynamic-reconfigure-param + "/move_base_node/global_costmap" "footprint_padding" + :double *global-footprint-padding*)) + (when (boundp '*local-footprint-padding*) + (ros::set-dynamic-reconfigure-param + "/move_base_node/local_costmap" "footprint_padding" + :double *local-footprint-padding*)) + (when (and (boundp '*look-at-human-enabled*) *look-at-human-enabled*) + (call-service "look_at_human/start" (instance std_srvs::EmptyRequest :init))) + t) diff --git a/elevator_move_base_pr2/test/gen_sample_data.l b/elevator_move_base_pr2/test/gen_sample_data.l index 3273cd45dd..62b54c3ad0 100755 --- a/elevator_move_base_pr2/test/gen_sample_data.l +++ b/elevator_move_base_pr2/test/gen_sample_data.l @@ -2,7 +2,8 @@ (ros::roseus "elevator_move_base") -(load "package://elevator_move_base_pr2/src/elevator-move-base-main.l") +(load "package://elevator_move_base_pr2/src/state-machine-main.l") +(load "package://elevator_move_base_pr2/src/utils.l") ;; ;; State Machine for test : push buttons @@ -11,7 +12,7 @@ ;; elevator_move_base action server (defun elevator-smach-initial (userdata) ;; goal -> args (update-robot-position) - (let ((cur-floor (check-current-floor))) + (let ((cur-floor (check-current-floor *scene* *tfl*))) (set-alist 'home-coords (send *ri* :state :worldcoords "/map") userdata) (set-alist 'outside-panel-name "/elevator_call_panel" userdata) (set-alist 'button-name "down" userdata) @@ -37,7 +38,7 @@ (send (send sm :node :go-elevator) :remap-list '((:goal . front-coords))) (send (send sm :node :go-home) :remap-list '((:goal . home-coords))) (send sm :arg-keys 'home-coords 'front-coords - 'outside-panel-name 'button-name :goal :cancel) + 'outside-panel-name 'button-name :goal) (send sm :goal-state (list :success :fail)) (send sm :start-state :initial) (send sm :add-transition :initial :speak-task t) @@ -53,6 +54,7 @@ ;; (initialize-env) +(ros::advertise "view_target" geometry_msgs::PointStamped 1) (setq sm (elevator-smach) userdata '(nil)) (ros::ros-info "initialized gen_sample_data.l") diff --git a/elevator_move_base_pr2/test/test-button-light-recognition.l b/elevator_move_base_pr2/test/test-button-light-recognition.l new file mode 100755 index 0000000000..8331a894bc --- /dev/null +++ b/elevator_move_base_pr2/test/test-button-light-recognition.l @@ -0,0 +1,25 @@ +#!/usr/bin/env roseus + +(load "lib/llib/unittest.l") +(load "package://elevator_move_base_pr2/src/state-machine-main.l") + + +(ros::roseus "test_number_recognition") + +(init-unit-test) + +(deftest test-number-recognition () + (let ((msg nil) (topic "light_button")) + (call-service "/wide_stereo/left/pass_through_image_rect_color/request" + (instance std_srvs::EmptyRequest :init)) + (ros::subscribe "light_button" std_msgs::float32 #'(lambda (m) (setq msg m))) + (ros::rate 10) + (while (ros::ok) + (ros::spin-once) + (when msg (return)) + (ros::sleep)) + (assert msg (format nil "Could not subscribe topic: ~A." topic)))) + +(run-all-tests) + +(exit) diff --git a/elevator_move_base_pr2/test/test-button-light.l b/elevator_move_base_pr2/test/test-button-light.l deleted file mode 100755 index ae30a87f55..0000000000 --- a/elevator_move_base_pr2/test/test-button-light.l +++ /dev/null @@ -1,30 +0,0 @@ -#!/usr/bin/env roseus - -(load "lib/llib/unittest.l") -(ros::load-ros-manifest "elevator_move_base_pr2") - -(ros::roseus "test-button-light") - -(init-unit-test) - -(defun button-callback (msg) - (ros::ros-info "button callback received: ~A" msg) - (setq *m* msg)) - -(deftest test-button-light () - (setq *m* nil count 0) - (ros::subscribe "light_button" std_msgs::float32 #'button-callback) - (ros::rate 1) - (while (and (ros::ok) (or (null *m*) (>= 1.0 (send *m* :data)))) - (ros::spin-once) - (ros::sleep)) - - (assert *m* - "detector node returns result") - (assert (and *m* (< 1.0 (send *m* :data))) - "button light has detected successfully") - ) - -(run-all-tests) - -(exit) diff --git a/elevator_move_base_pr2/test/test-button-light.launch b/elevator_move_base_pr2/test/test-button-light.launch index a8cf3e6706..ed94f74d95 100644 --- a/elevator_move_base_pr2/test/test-button-light.launch +++ b/elevator_move_base_pr2/test/test-button-light.launch @@ -1,61 +1,25 @@ - - - + - - - - - - - + - - - - - - - - - - - - - + + + + + - - - + + diff --git a/elevator_move_base_pr2/test/test-button-light.vcg b/elevator_move_base_pr2/test/test-button-light.vcg deleted file mode 100644 index 3359c975a0..0000000000 --- a/elevator_move_base_pr2/test/test-button-light.vcg +++ /dev/null @@ -1,53 +0,0 @@ -Background\ ColorR=0 -Background\ ColorG=0 -Background\ ColorB=0 -Fixed\ Frame=/base_link -Target\ Frame= -Grid.Alpha=0.5 -Grid.Cell\ Size=1 -Grid.ColorR=0.5 -Grid.ColorG=0.5 -Grid.ColorB=0.5 -Grid.Enabled=1 -Grid.Line\ Style=0 -Grid.Line\ Width=0.03 -Grid.Normal\ Cell\ Count=0 -Grid.OffsetX=0 -Grid.OffsetY=0 -Grid.OffsetZ=0 -Grid.Plane=0 -Grid.Plane\ Cell\ Count=10 -Grid.Reference\ Frame= -Image.Enabled=1 -Image.Image\ Topic=/light_detector/debug_image -Image.Transport\ Hint=raw -MarkerArray.Enabled=1 -MarkerArray.Marker\ Array\ Topic=/openrave_marker_array -Robot\ Model.Alpha=1 -Robot\ Model.Collision\ Enabled=0 -Robot\ Model.Enabled=1 -Robot\ Model.Robot\ Description=robot_description -Robot\ Model.TF\ Prefix= -Robot\ Model.Update\ Interval=0 -Robot\ Model.Visual\ Enabled=1 -Tool\ 2D\ Nav\ GoalTopic=move -Tool\ 2D\ Pose\ EstimateTopic=initialpose -Camera\ Type=rviz::XYOrbitViewController -Camera\ Config=1.346 3.70356 2.98831 0.456939 1.31131e-05 0.0791044 -Property\ Grid\ State=selection=Robot Model.Enabled;expanded=.Global Options,MarkerArray.Enabled.MarkerArray.Namespaces,Image2.Enabled;scrollpos=0,0;splitterpos=241,499;ispageselected=1 -[Display0] -Name=Grid -Package=rviz -ClassName=rviz::GridDisplay -[Display1] -Name=MarkerArray -Package=rviz -ClassName=rviz::MarkerArrayDisplay -[Display2] -Name=Robot Model -Package=rviz -ClassName=rviz::RobotModelDisplay -[Display3] -Name=Image -Package=rviz -ClassName=rviz::ImageDisplay diff --git a/elevator_move_base_pr2/test/test-color-point-detector.bag b/elevator_move_base_pr2/test/test-color-point-detector.bag new file mode 100644 index 0000000000..4b196c4e91 Binary files /dev/null and b/elevator_move_base_pr2/test/test-color-point-detector.bag differ diff --git a/elevator_move_base_pr2/test/test-color-point-detector.launch b/elevator_move_base_pr2/test/test-color-point-detector.launch index f1f0518482..6b62dae59c 100644 --- a/elevator_move_base_pr2/test/test-color-point-detector.launch +++ b/elevator_move_base_pr2/test/test-color-point-detector.launch @@ -1,33 +1,49 @@ - - + - - - - - - + + + - - + + + + red: 253 + green: 251 + blue: 240 + decay_r: 0.1 + decay_g: 0.1 + decay_b: 0.05 + + - - - - - + + + topic: /light_button + hz: 2.0 + hzerror: 0.5 + test_duration: 2.0 + + - + + + target_value: 1.5 + timeout: 3.0 + + diff --git a/elevator_move_base_pr2/test/test-color-point-detector.py b/elevator_move_base_pr2/test/test-color-point-detector.py index 3f5b35cc5f..69c62d568d 100755 --- a/elevator_move_base_pr2/test/test-color-point-detector.py +++ b/elevator_move_base_pr2/test/test-color-point-detector.py @@ -1,25 +1,38 @@ #!/usr/bin/env python -PKG = 'elevator_move_base_pr2' -import roslib; roslib.load_manifest(PKG) -import sys, unittest, time, rospy +import time +import unittest + +import rospy +import rostest from std_msgs.msg import Float32 -TEST_DURATION = 3 -## A sample python unit test class TestColorPointDetector(unittest.TestCase): + + def setUp(self): + self.timeout = rospy.get_param('~timeout', 3.0) + self.target_value = rospy.get_param('~target_value', 1.5) + self.msg = None + self.sub = rospy.Subscriber('/light_button', Float32, self.callback) + def callback(self, msg): - print "msg.data = ", msg.data, " should be 1.5" - self.assert_( abs( msg.data - 1.5 ) < 0.1 ) - + self.msg = msg + def test_light_button(self): - rospy.init_node(PKG, anonymous=True) - sub = rospy.Subscriber("/light_button", Float32, self.callback) - timeout_t = time.time() + TEST_DURATION + timeout_t = time.time() + self.timeout while not rospy.is_shutdown() and time.time() < timeout_t: + if self.msg is not None: + rospy.loginfo( + 'msg.data should be {}, actual: {}'.format( + self.target_value, self.msg.data)) + self.assertTrue(abs(self.msg.data - self.target_value) < 0.1) + return time.sleep(0.1) + self.fail('Test timeout.') + if __name__ == '__main__': - import rostest - rostest.rosrun(PKG, 'test_data', TestColorPointDetector) + rospy.init_node('test_color_point_detector') + rostest.rosrun('elevator_move_base_pr2', 'test_color_point_detector', + TestColorPointDetector) diff --git a/elevator_move_base_pr2/test/test-eng2-call-panel.bag b/elevator_move_base_pr2/test/test-eng2-call-panel.bag new file mode 100644 index 0000000000..1248ad3f7c Binary files /dev/null and b/elevator_move_base_pr2/test/test-eng2-call-panel.bag differ diff --git a/elevator_move_base_pr2/test/test-eng2-inside-panel.bag b/elevator_move_base_pr2/test/test-eng2-inside-panel.bag new file mode 100644 index 0000000000..66508d5f96 Binary files /dev/null and b/elevator_move_base_pr2/test/test-eng2-inside-panel.bag differ diff --git a/elevator_move_base_pr2/test/test-modules-callpanel.bag b/elevator_move_base_pr2/test/test-modules-callpanel.bag deleted file mode 100644 index 3f20e11f5d..0000000000 Binary files a/elevator_move_base_pr2/test/test-modules-callpanel.bag and /dev/null differ diff --git a/elevator_move_base_pr2/test/test-modules-callpanel.l b/elevator_move_base_pr2/test/test-modules-callpanel.l deleted file mode 100755 index f658d40379..0000000000 --- a/elevator_move_base_pr2/test/test-modules-callpanel.l +++ /dev/null @@ -1,56 +0,0 @@ -#!/usr/bin/env roseus - -(load "lib/llib/unittest.l") -(ros::load-ros-manifest "elevator_move_base_pr2") -(load "package://pr2eus/pr2-interface.l") -(load "package://elevator_move_base_pr2/src/posedetectiondb-client.l") -(load "package://elevator_move_base_pr2/src/elevator-move-base-main.l") -(ros::roseus "test_modules_callpanel") - -(init-unit-test) - -(deftest test-modules-callpanel () - (ros::advertise "visualization_marker" visualization_msgs::Marker) - (ros::advertise "view_target" geometry_msgs::PointStamped 1) - (pr2) - (setq *ri* (instance pr2-interface :init)) - (setq count 0 - *m* nil - *panel* (instance elevator_call_panel_eng2-object :init - :name "elevator_call_panel") - button (find "down" (send *panel* :button) :test #'equal - :key #'(lambda(x)(send x :name))) - marker (sphere->marker-msg (make-sphere 50) - (instance std_msgs::header :init :frame_id "/world"))) - ;; panel pose - (setq client (instance posedetectiondb-client :init *panel* - :fixed-frame "/world" - :topic "/narrow_stereo/left/ObjectDetection")) - (ros::subscribe "light_button" std_msgs::float32 #'(lambda(m)(setq *m* m))) - ;; - (while (and (< count 100) (not (and *m* (< 1.0 (send *m* :data))))) - (ros::spin-once) - ;; set button area - (send *pr2* :move-to (send *ri* :state :worldcoords) :world) - (send *pr2* :angle-vector (send *ri* :state)) - (send marker :pose (ros::coords->tf-pose (send button :copy-worldcoords))) - (send marker :color (vector->rgba #(0 0 1) 1)) ;; blue - (ros::publish "visualization_marker" marker) - (set-view-target (send button :copy-worldcoords) - (send *pr2* :wide_stereo-left) - "wide_stereo_optical_frame") - ;; get button state - (unix::usleep (* 100 1000))) - ;; finish and check - (send marker :color (vector->rgba #(1 0 0) 1)) ;; red - (ros::publish "visualization_marker" marker) - (assert *m* - "detector node returns result") - (assert (and *m* (< 1.0 (send *m* :data))) - "button light has detected successfully") - (unix::sleep 15) ;; for making video - ) - -(run-all-tests) - -(exit) diff --git a/elevator_move_base_pr2/test/test-modules-callpanel.launch b/elevator_move_base_pr2/test/test-modules-callpanel.launch index 128268cc4e..729d3d58d0 100644 --- a/elevator_move_base_pr2/test/test-modules-callpanel.launch +++ b/elevator_move_base_pr2/test/test-modules-callpanel.launch @@ -1,47 +1,39 @@ - - - + - - - - - - - - - - - - + + + + + + + + + + + + - - - - - - + + + - - + + + + panel_type: elevator_call_panel + + diff --git a/elevator_move_base_pr2/test/test-modules-callpanel.vcg b/elevator_move_base_pr2/test/test-modules-callpanel.vcg deleted file mode 100644 index f60dafa04c..0000000000 --- a/elevator_move_base_pr2/test/test-modules-callpanel.vcg +++ /dev/null @@ -1,165 +0,0 @@ -Background\ ColorR=0 -Background\ ColorG=0 -Background\ ColorB=0 -Fixed\ Frame=/map -Target\ Frame=/base_link -Grid.Alpha=0.5 -Grid.Cell\ Size=1 -Grid.ColorR=0.5 -Grid.ColorG=0.5 -Grid.ColorB=0.5 -Grid.Enabled=1 -Grid.Line\ Style=0 -Grid.Line\ Width=0.03 -Grid.Normal\ Cell\ Count=0 -Grid.OffsetX=0 -Grid.OffsetY=0 -Grid.OffsetZ=0 -Grid.Plane=0 -Grid.Plane\ Cell\ Count=10 -Grid.Reference\ Frame= -Image.Enabled=1 -Image.Image\ Topic=/wide_stereo/left/image_rect_color -Image.Transport\ Hint=raw -Map.Alpha=0.7 -Map.Draw\ Behind=0 -Map.Enabled=1 -Map.Topic=/map -Marker.Enabled=1 -Marker.Marker\ Topic=visualization_marker -Robot\ Model.Alpha=1 -Robot\ Model.Collision\ Enabled=0 -Robot\ Model.Enabled=1 -Robot\ Model.Robot\ Description=robot_description -Robot\ Model.TF\ Prefix= -Robot\ Model.Update\ Interval=0 -Robot\ Model.Visual\ Enabled=1 -Robot\:\ Robot\ Model\ Link\ base_bellow_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_bellow_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ base_footprintShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_footprintShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ double_stereo_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ double_stereo_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_motor_accelerometer_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_motor_accelerometer_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_motor_accelerometer_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_motor_accelerometer_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ sensor_mount_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ sensor_mount_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Trail=0 -Tool\ 2D\ Nav\ GoalTopic=move -Tool\ 2D\ Pose\ EstimateTopic=initialpose -Camera\ Type=rviz::XYOrbitViewController -Camera\ Config=1.221 5.23356 3.07856 0.412825 1.31131e-05 -0.267637 -Property\ Grid\ State=selection=;expanded=.Global Options,Image.Enabled,Marker.Enabled,Marker.Enabled.Marker.Namespaces;scrollpos=0,0;splitterpos=150,285;ispageselected=1 -[Display0] -Name=Grid -Package=rviz -ClassName=rviz::GridDisplay -[Display1] -Name=Robot Model -Package=rviz -ClassName=rviz::RobotModelDisplay -[Display2] -Name=Image -Package=rviz -ClassName=rviz::ImageDisplay -[Display3] -Name=Map -Package=rviz -ClassName=rviz::MapDisplay -[Display4] -Name=Marker -Package=rviz -ClassName=rviz::MarkerDisplay diff --git a/elevator_move_base_pr2/test/test-modules-insidepanel.launch b/elevator_move_base_pr2/test/test-modules-insidepanel.launch index 400d6ac5a5..32fae25850 100644 --- a/elevator_move_base_pr2/test/test-modules-insidepanel.launch +++ b/elevator_move_base_pr2/test/test-modules-insidepanel.launch @@ -1,45 +1,41 @@ - - - + - - - - - - - - - - - - - - -.. video:: build/images/call-panel-pose - :width: 600 + + -Then apply affine transform to camera image for template match. -Template is number region of the panel. + + + + + + + -.. video:: build/images/inside-panel-number - :width: 600 - - ]]> + + + - + + + + panel_type: elevator_inside_panel + + - - - + + diff --git a/elevator_move_base_pr2/test/test-number-recognition.l b/elevator_move_base_pr2/test/test-number-recognition.l index a0ab610ed6..279c6db374 100755 --- a/elevator_move_base_pr2/test/test-number-recognition.l +++ b/elevator_move_base_pr2/test/test-number-recognition.l @@ -1,16 +1,24 @@ #!/usr/bin/env roseus (load "lib/llib/unittest.l") -(ros::load-ros-manifest "elevator_move_base_pr2") +(load "package://elevator_move_base_pr2/src/state-machine-main.l") -(ros::roseus "test-number-recognition") -(load "package://elevator_move_base_pr2/src/elevator-move-base-main.l") +(ros::roseus "test_number_recognition") (init-unit-test) -(deftest test-dummy () - (assert t)) +(deftest test-number-recognition () + (let ((msg nil) (topic "/elevator_number/result")) + (call-service "/narrow_stereo/left/pass_through_image_rect/request" + (instance std_srvs::EmptyRequest :init)) + (ros::subscribe topic roseus::StringStamped #'(lambda (m) (setq msg m))) + (ros::rate 10) + (while (ros::ok) + (ros::spin-once) + (when msg (return)) + (ros::sleep)) + (assert msg (format nil "Could not subscribe topic: ~A." topic)))) (run-all-tests) diff --git a/elevator_move_base_pr2/test/test-panel-pose-detection.l b/elevator_move_base_pr2/test/test-panel-pose-detection.l index a7c8417fe5..ab4b7747ec 100755 --- a/elevator_move_base_pr2/test/test-panel-pose-detection.l +++ b/elevator_move_base_pr2/test/test-panel-pose-detection.l @@ -1,52 +1,60 @@ #!/usr/bin/env roseus -(load "lib/llib/unittest.l") (ros::roseus-add-msgs "posedetection_msgs") -(ros::roseus "test-panel-pose-detection") -(defvar *tfl* (instance ros::transform-listener :init)) -(setq *msg* nil) +(require :unittest "lib/llib/unittest.l") + (init-unit-test) -(defun object-detection-cb (msg) - (ros::ros-info "*msg*: ~A" (setq *msg* msg))) - -(deftest test-call-panel () - (ros::subscribe "/narrow_stereo/left/ObjectDetection" - posedetection_msgs::ObjectDetection #'object-detection-cb) - (setq is-ok nil) - (setq *distance* -1) - (ros::rate 1) - (dotimes (cnt 30) ;; 30sec in real - (when *msg* - (let (co tf) + +(deftest test-elevator-panel () + (let ((tfl (instance ros::transform-listener :init)) + (dist -1) + (max-dist (ros::get-param "~max_distance" 50)) + (panel-type (ros::get-param "~panel_type" "elevator_call_panel")) + co1 co2 + (msg nil)) + (unix:usleep (* 100 1000)) ;; Wait for service server + (if (equal panel-type "elevator_call_panel") + (call-empty-service + "/narrow_stereo/left/pass_through_feature_to_call_panel/request") + (call-empty-service + "/narrow_stereo/left/pass_through_feature_to_inside_panel/request")) + (ros::subscribe "/narrow_stereo/left/ObjectDetection" + posedetection_msgs::ObjectDetection + #'(lambda (m) (setq msg m))) + + (ros::rate 1) + (dotimes (cnt 30) ;; 30sec in ros time + (ros::spin-once) + (when msg (setq co1 (ros::tf-pose->coords - (send (car (send *msg* :objects)) :pose))) - (setq co2 (send *tfl* :lookup-transform - (send *msg* :header :frame_id) - (send (car (send *msg* :objects)) :type) - (send *msg* :header :stamp))) + (send (car (send msg :objects)) :pose))) + (setq co2 (send tfl :lookup-transform + (send msg :header :frame_id) + (concatenate string "/" panel-type) + (ros::time 0))) (when (and co1 co2) + (setq dist (distance (send co1 :pos) (send co2 :pos))) (ros::ros-info "co1: ~A" co1) (ros::ros-info "co2: ~A" co2) - (ros::ros-info "dist: ~A" (distance (send co1 :pos) (send co2 :pos))) - (setq *distance* (distance (send co1 :pos) (send co2 :pos))) - (when (> 50 (distance (send co1 :pos) (send co2 :pos))) - (return) - )) - (setq *msg* nil))) - (ros::spin-once) - (ros::sleep)) - (assert *msg* "/narrow_stereo/left/ObjectDetection not received") - (assert (> *distance* 0) - (format nil "looking transform not found")) - (assert (< *distance* 50) - (format nil - "ObjectDetection and TF(in bag) are close: ~A" *distance*)) - (unix::sleep 15) ;; for making video - ) + (ros::ros-info "dist: ~A" dist) + (when (> max-dist dist) + (return))) + (setq msg nil)) + (ros::sleep)) -(run-all-tests) + (assert msg "Could not receive /narrow_stereo/left/ObjectDetection") + (assert (> dist 0) + (format nil "looking transform not found")) + (assert (< dist max-dist) + (format nil "ObjectDetection and TF(in bag) are not close: ~A" + dist)) + )) + +(ros::roseus "test_panel_pose_detection") + +(run-all-tests) (exit) diff --git a/elevator_move_base_pr2/test/test-panel-pose-detection.launch b/elevator_move_base_pr2/test/test-panel-pose-detection.launch index 25a812892f..2fcf4b0255 100644 --- a/elevator_move_base_pr2/test/test-panel-pose-detection.launch +++ b/elevator_move_base_pr2/test/test-panel-pose-detection.launch @@ -1,56 +1,72 @@ - + + - + + - - - - - - - - + + - + - - - - - - + + + queue_size: 100 + - - + + + + + + + + + update_rate: 2.0 + + + + + + + + + + + + + error_threshold: 500 + + - - - - - - - - + + + panel_type: elevator_call_panel + max_distance: 100 + + diff --git a/elevator_move_base_pr2/test/test-panel-pose-detection.vcg b/elevator_move_base_pr2/test/test-panel-pose-detection.vcg deleted file mode 100644 index 754f8c85a0..0000000000 --- a/elevator_move_base_pr2/test/test-panel-pose-detection.vcg +++ /dev/null @@ -1,283 +0,0 @@ -Background\ ColorR=0 -Background\ ColorG=0 -Background\ ColorB=0 -Fixed\ Frame=/base_link -Target\ Frame= -Grid.Alpha=0.5 -Grid.Cell\ Size=1 -Grid.ColorR=0.5 -Grid.ColorG=0.5 -Grid.ColorB=0.5 -Grid.Enabled=1 -Grid.Line\ Style=0 -Grid.Line\ Width=0.03 -Grid.Normal\ Cell\ Count=0 -Grid.OffsetX=0 -Grid.OffsetY=0 -Grid.OffsetZ=0 -Grid.Plane=0 -Grid.Plane\ Cell\ Count=10 -Grid.Reference\ Frame= -Image.Enabled=1 -Image.Image\ Topic=/narrow_stereo/left/point_pose_extractor_elevator_call_panel/debug_image -Image.Transport\ Hint=raw -Map.Alpha=0.7 -Map.Draw\ Behind=0 -Map.Enabled=1 -Map.Topic=/map -MarkerArray.Enabled=1 -MarkerArray.Marker\ Array\ Topic=/openrave_marker_array -Robot\ Model.Alpha=1 -Robot\ Model.Collision\ Enabled=0 -Robot\ Model.Enabled=1 -Robot\ Model.Robot\ Description=robot_description -Robot\ Model.TF\ Prefix= -Robot\ Model.Update\ Interval=0 -Robot\ Model.Visual\ Enabled=1 -Robot\:\ Robot\ Model\ Link\ base_bellow_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_bellow_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ base_footprintShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_footprintShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ bl_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ br_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ br_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ br_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ double_stereo_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ double_stereo_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fl_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_l_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_r_wheel_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ fr_caster_rotation_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ head_pan_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ head_plate_frameShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ head_tilt_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_elbow_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_forearm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_l_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_motor_accelerometer_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_motor_accelerometer_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_palm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_gripper_r_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_lift_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_shoulder_pan_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_upper_arm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ l_wrist_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ laser_tilt_mount_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_elbow_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_forearm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_l_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_motor_accelerometer_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_motor_accelerometer_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_palm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_gripper_r_finger_tip_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_lift_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_shoulder_pan_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_upper_arm_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_flex_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ r_wrist_roll_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ sensor_mount_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ sensor_mount_linkShow\ Trail=0 -Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Axes=0 -Robot\:\ Robot\ Model\ Link\ torso_lift_linkShow\ Trail=0 -TF.Enabled=1 -TF.Frame\ Timeout=15 -TF.Marker\ Scale=1 -TF.Show\ Arrows=0 -TF.Show\ Axes=1 -TF.Show\ Names=0 -TF.Update\ Interval=0.05 -Tool\ 2D\ Nav\ GoalTopic=move -Tool\ 2D\ Pose\ EstimateTopic=initialpose -Camera\ Type=rviz::XYOrbitViewController -Camera\ Config=1.241 5.02356 23.36 23.4804 1.92414e-05 -63.1444 -Property\ Grid\ State=selection=;expanded=.Global Options,MarkerArray.Enabled.MarkerArray.Namespaces,TF./base_bellow_link/base_bellow_link,TF./base_footprint/base_footprint,TF./base_laser_link/base_laser_link,TF./base_link/base_link,TF./bl_caster_l_wheel_link/bl_caster_l_wheel_link,TF./bl_caster_r_wheel_link/bl_caster_r_wheel_link,TF./bl_caster_rotation_link/bl_caster_rotation_link,TF./br_caster_l_wheel_link/br_caster_l_wheel_link,TF./br_caster_r_wheel_link/br_caster_r_wheel_link,TF./br_caster_rotation_link/br_caster_rotation_link,TF./double_stereo_link/double_stereo_link,TF./elevator_call_panel/elevator_call_panel,TF./eng2/eng2,TF./eng2/2f/eng2/2f,TF./eng2/3f/eng2/3f,TF./eng2/7f/eng2/7f,TF./eng2/7f/73B2/eng2/7f/73B2,TF./eng2/8f/eng2/8f,TF./fl_caster_l_wheel_link/fl_caster_l_wheel_link,TF./fl_caster_r_wheel_link/fl_caster_r_wheel_link,TF./fl_caster_rotation_link/fl_caster_rotation_link,TF./fr_caster_l_wheel_link/fr_caster_l_wheel_link,TF./fr_caster_r_wheel_link/fr_caster_r_wheel_link,TF./fr_caster_rotation_link/fr_caster_rotation_link,TF./head_pan_link/head_pan_link,TF./head_plate_frame/head_plate_frame,TF./head_tilt_link/head_tilt_link,TF./high_def_frame/high_def_frame,TF./high_def_optical_frame/high_def_optical_frame,TF./imu_link/imu_link,TF./kinect_link/kinect_link,TF./l_elbow_flex_link/l_elbow_flex_link,TF./l_forearm_cam_frame/l_forearm_cam_frame,TF./l_forearm_cam_optical_frame/l_forearm_cam_optical_frame,TF./l_forearm_link/l_forearm_link,TF./l_forearm_roll_link/l_forearm_roll_link,TF./l_gripper_l_finger_link/l_gripper_l_finger_link,TF./l_gripper_l_finger_tip_frame/l_gripper_l_finger_tip_frame,TF./l_gripper_l_finger_tip_link/l_gripper_l_finger_tip_link,TF./l_gripper_led_frame/l_gripper_led_frame,TF./l_gripper_motor_accelerometer_link/l_gripper_motor_accelerometer_link,TF./l_gripper_motor_screw_link/l_gripper_motor_screw_link,TF./l_gripper_motor_slider_link/l_gripper_motor_slider_link,TF./l_gripper_palm_link/l_gripper_palm_link,TF./l_gripper_r_finger_link/l_gripper_r_finger_link,TF./l_gripper_r_finger_tip_link/l_gripper_r_finger_tip_link,TF./l_gripper_tool_frame/l_gripper_tool_frame,TF./l_shoulder_lift_link/l_shoulder_lift_link,TF./l_shoulder_pan_link/l_shoulder_pan_link,TF./l_torso_lift_side_plate_link/l_torso_lift_side_plate_link,TF./l_upper_arm_link/l_upper_arm_link,TF./l_upper_arm_roll_link/l_upper_arm_roll_link,TF./l_wrist_flex_link/l_wrist_flex_link,TF./l_wrist_roll_link/l_wrist_roll_link,TF./laser_tilt_link/laser_tilt_link,TF./laser_tilt_mount_link/laser_tilt_mount_link,TF./map/map,TF./narrow_stereo_gazebo_l_stereo_camera_frame/narrow_stereo_gazebo_l_stereo_camera_frame,TF./narrow_stereo_gazebo_l_stereo_camera_optical_frame/narrow_stereo_gazebo_l_stereo_camera_optical_frame,TF./narrow_stereo_gazebo_r_stereo_camera_frame/narrow_stereo_gazebo_r_stereo_camera_frame,TF./narrow_stereo_gazebo_r_stereo_camera_optical_frame/narrow_stereo_gazebo_r_stereo_camera_optical_frame,TF./narrow_stereo_link/narrow_stereo_link,TF./narrow_stereo_optical_frame/narrow_stereo_optical_frame,TF./odom_combined/odom_combined,TF./openni_camera/openni_camera,TF./openni_depth_frame/openni_depth_frame,TF./openni_depth_optical_frame/openni_depth_optical_frame,TF./openni_rgb_frame/openni_rgb_frame,TF./openni_rgb_optical_frame/openni_rgb_optical_frame,TF./projector_wg6802418_child_frame/projector_wg6802418_child_frame,TF./projector_wg6802418_frame/projector_wg6802418_frame,TF./r_elbow_flex_link/r_elbow_flex_link,TF./r_forearm_cam_frame/r_forearm_cam_frame,TF./r_forearm_cam_optical_frame/r_forearm_cam_optical_frame,TF./r_forearm_link/r_forearm_link,TF./r_forearm_roll_link/r_forearm_roll_link,TF./r_gripper_l_finger_link/r_gripper_l_finger_link,TF./r_gripper_l_finger_tip_frame/r_gripper_l_finger_tip_frame,TF./r_gripper_l_finger_tip_link/r_gripper_l_finger_tip_link,TF./r_gripper_led_frame/r_gripper_led_frame,TF./r_gripper_motor_accelerometer_link/r_gripper_motor_accelerometer_link,TF./r_gripper_motor_screw_link/r_gripper_motor_screw_link,TF./r_gripper_motor_slider_link/r_gripper_motor_slider_link,TF./r_gripper_palm_link/r_gripper_palm_link,TF./r_gripper_r_finger_link/r_gripper_r_finger_link,TF./r_gripper_r_finger_tip_link/r_gripper_r_finger_tip_link,TF./r_gripper_tool_frame/r_gripper_tool_frame,TF./r_shoulder_lift_link/r_shoulder_lift_link,TF./r_shoulder_pan_link/r_shoulder_pan_link,TF./r_torso_lift_side_plate_link/r_torso_lift_side_plate_link,TF./r_upper_arm_link/r_upper_arm_link,TF./r_upper_arm_roll_link/r_upper_arm_roll_link,TF./r_wrist_flex_link/r_wrist_flex_link,TF./r_wrist_roll_link/r_wrist_roll_link,TF./sensor_mount_link/sensor_mount_link,TF./torso_lift_link/torso_lift_link,TF./torso_lift_motor_screw_link/torso_lift_motor_screw_link,TF./wide_stereo_gazebo_l_stereo_camera_frame/wide_stereo_gazebo_l_stereo_camera_frame,TF./wide_stereo_gazebo_l_stereo_camera_optical_frame/wide_stereo_gazebo_l_stereo_camera_optical_frame,TF./wide_stereo_gazebo_r_stereo_camera_frame/wide_stereo_gazebo_r_stereo_camera_frame,TF./wide_stereo_gazebo_r_stereo_camera_optical_frame/wide_stereo_gazebo_r_stereo_camera_optical_frame,TF./wide_stereo_link/wide_stereo_link,TF./wide_stereo_optical_frame/wide_stereo_optical_frame,TF./world/world,TF./worldTree/world,TF./eng2Tree/eng2,TF./eng2/2fTree/eng2/2f,TF./eng2/3fTree/eng2/3f,TF./eng2/7fTree/eng2/7f,TF./eng2/7f/73B2Tree/eng2/7f/73B2,TF./mapTree/map,TF./odom_combinedTree/odom_combined,TF./base_footprintTree/base_footprint,TF./base_linkTree/base_link,TF./bl_caster_rotation_linkTree/bl_caster_rotation_link,TF./bl_caster_l_wheel_linkTree/bl_caster_l_wheel_link,TF./bl_caster_r_wheel_linkTree/bl_caster_r_wheel_link,TF./br_caster_rotation_linkTree/br_caster_rotation_link,TF./br_caster_l_wheel_linkTree/br_caster_l_wheel_link,TF./br_caster_r_wheel_linkTree/br_caster_r_wheel_link,TF./fl_caster_rotation_linkTree/fl_caster_rotation_link,TF./fl_caster_l_wheel_linkTree/fl_caster_l_wheel_link,TF./fl_caster_r_wheel_linkTree/fl_caster_r_wheel_link,TF./fr_caster_rotation_linkTree/fr_caster_rotation_link,TF./fr_caster_l_wheel_linkTree/fr_caster_l_wheel_link,TF./fr_caster_r_wheel_linkTree/fr_caster_r_wheel_link,TF./torso_lift_linkTree/torso_lift_link,TF./head_pan_linkTree/head_pan_link,TF./head_tilt_linkTree/head_tilt_link,TF./head_plate_frameTree/head_plate_frame,TF./kinect_linkTree/kinect_link,TF./openni_cameraTree/openni_camera,TF./openni_depth_frameTree/openni_depth_frame,TF./openni_depth_optical_frameTree/openni_depth_optical_frame,TF./openni_rgb_frameTree/openni_rgb_frame,TF./openni_rgb_optical_frameTree/openni_rgb_optical_frame,TF./projector_wg6802418_frameTree/projector_wg6802418_frame,TF./projector_wg6802418_child_frameTree/projector_wg6802418_child_frame,TF./sensor_mount_linkTree/sensor_mount_link,TF./double_stereo_linkTree/double_stereo_link,TF./narrow_stereo_linkTree/narrow_stereo_link,TF./narrow_stereo_optical_frameTree/narrow_stereo_optical_frame,TF./narrow_stereo_gazebo_l_stereo_camera_frameTree/narrow_stereo_gazebo_l_stereo_camera_frame,TF./narrow_stereo_gazebo_l_stereo_camera_optical_frameTree/narrow_stereo_gazebo_l_stereo_camera_optical_frame,TF./narrow_stereo_gazebo_r_stereo_camera_frameTree/narrow_stereo_gazebo_r_stereo_camera_frame,TF./narrow_stereo_gazebo_r_stereo_camera_optical_frameTree/narrow_stereo_gazebo_r_stereo_camera_optical_frame,TF./wide_stereo_linkTree/wide_stereo_link,TF./wide_stereo_optical_frameTree/wide_stereo_optical_frame,TF./wide_stereo_gazebo_l_stereo_camera_frameTree/wide_stereo_gazebo_l_stereo_camera_frame,TF./wide_stereo_gazebo_l_stereo_camera_optical_frameTree/wide_stereo_gazebo_l_stereo_camera_optical_frame,TF./wide_stereo_gazebo_r_stereo_camera_frameTree/wide_stereo_gazebo_r_stereo_camera_frame,TF./wide_stereo_gazebo_r_stereo_camera_optical_frameTree/wide_stereo_gazebo_r_stereo_camera_optical_frame,TF./high_def_frameTree/high_def_frame,TF./high_def_optical_frameTree/high_def_optical_frame,TF./imu_linkTree/imu_link,TF./l_shoulder_pan_linkTree/l_shoulder_pan_link,TF./l_shoulder_lift_linkTree/l_shoulder_lift_link,TF./l_upper_arm_roll_linkTree/l_upper_arm_roll_link,TF./l_upper_arm_linkTree/l_upper_arm_link,TF./l_elbow_flex_linkTree/l_elbow_flex_link,TF./l_forearm_roll_linkTree/l_forearm_roll_link,TF./l_forearm_cam_frameTree/l_forearm_cam_frame,TF./l_forearm_cam_optical_frameTree/l_forearm_cam_optical_frame,TF./l_forearm_linkTree/l_forearm_link,TF./l_wrist_flex_linkTree/l_wrist_flex_link,TF./l_wrist_roll_linkTree/l_wrist_roll_link,TF./l_gripper_palm_linkTree/l_gripper_palm_link,TF./l_gripper_r_finger_linkTree/l_gripper_r_finger_link,TF./l_gripper_r_finger_tip_linkTree/l_gripper_r_finger_tip_link,TF./l_gripper_l_finger_tip_frameTree/l_gripper_l_finger_tip_frame,TF./l_gripper_tool_frameTree/l_gripper_tool_frame,TF./l_gripper_l_finger_linkTree/l_gripper_l_finger_link,TF./l_gripper_l_finger_tip_linkTree/l_gripper_l_finger_tip_link,TF./l_gripper_led_frameTree/l_gripper_led_frame,TF./l_gripper_motor_accelerometer_linkTree/l_gripper_motor_accelerometer_link,TF./l_gripper_motor_slider_linkTree/l_gripper_motor_slider_link,TF./l_gripper_motor_screw_linkTree/l_gripper_motor_screw_link,TF./l_torso_lift_side_plate_linkTree/l_torso_lift_side_plate_link,TF./laser_tilt_mount_linkTree/laser_tilt_mount_link,TF./laser_tilt_linkTree/laser_tilt_link,TF./r_shoulder_pan_linkTree/r_shoulder_pan_link,TF./r_shoulder_lift_linkTree/r_shoulder_lift_link,TF./r_upper_arm_roll_linkTree/r_upper_arm_roll_link,TF./r_upper_arm_linkTree/r_upper_arm_link,TF./r_elbow_flex_linkTree/r_elbow_flex_link,TF./r_forearm_roll_linkTree/r_forearm_roll_link,TF./r_forearm_cam_frameTree/r_forearm_cam_frame,TF./r_forearm_cam_optical_frameTree/r_forearm_cam_optical_frame,TF./r_forearm_linkTree/r_forearm_link,TF./r_wrist_flex_linkTree/r_wrist_flex_link,TF./r_wrist_roll_linkTree/r_wrist_roll_link,TF./r_gripper_palm_linkTree/r_gripper_palm_link,TF./r_gripper_r_finger_linkTree/r_gripper_r_finger_link,TF./r_gripper_r_finger_tip_linkTree/r_gripper_r_finger_tip_link,TF./r_gripper_l_finger_tip_frameTree/r_gripper_l_finger_tip_frame,TF./r_gripper_tool_frameTree/r_gripper_tool_frame,TF./r_gripper_l_finger_linkTree/r_gripper_l_finger_link,TF./r_gripper_l_finger_tip_linkTree/r_gripper_l_finger_tip_link,TF./r_gripper_led_frameTree/r_gripper_led_frame,TF./r_gripper_motor_accelerometer_linkTree/r_gripper_motor_accelerometer_link,TF./r_gripper_motor_slider_linkTree/r_gripper_motor_slider_link,TF./r_gripper_motor_screw_linkTree/r_gripper_motor_screw_link,TF./r_torso_lift_side_plate_linkTree/r_torso_lift_side_plate_link,TF./torso_lift_motor_screw_linkTree/torso_lift_motor_screw_link,TF./base_bellow_linkTree/base_bellow_link,TF./base_laser_linkTree/base_laser_link,TF./elevator_call_panelTree/elevator_call_panel,TF./eng2/8fTree/eng2/8f;scrollpos=0,0;splitterpos=150,301;ispageselected=1 -[Display0] -Name=Grid -Package=rviz -ClassName=rviz::GridDisplay -[Display1] -Name=MarkerArray -Package=rviz -ClassName=rviz::MarkerArrayDisplay -[Display2] -Name=Robot Model -Package=rviz -ClassName=rviz::RobotModelDisplay -[Display3] -Name=Image -Package=rviz -ClassName=rviz::ImageDisplay -[Display4] -Name=Map -Package=rviz -ClassName=rviz::MapDisplay -[Display5] -Name=TF -Package=rviz -ClassName=rviz::TFDisplay -[TF.] -base_bellow_linkEnabled=1 -base_footprintEnabled=1 -base_laser_linkEnabled=1 -base_linkEnabled=1 -bl_caster_l_wheel_linkEnabled=1 -bl_caster_r_wheel_linkEnabled=1 -bl_caster_rotation_linkEnabled=1 -br_caster_l_wheel_linkEnabled=1 -br_caster_r_wheel_linkEnabled=1 -br_caster_rotation_linkEnabled=1 -double_stereo_linkEnabled=1 -elevator_call_panelEnabled=1 -eng2Enabled=1 -fl_caster_l_wheel_linkEnabled=1 -fl_caster_r_wheel_linkEnabled=1 -fl_caster_rotation_linkEnabled=1 -fr_caster_l_wheel_linkEnabled=1 -fr_caster_r_wheel_linkEnabled=1 -fr_caster_rotation_linkEnabled=1 -head_pan_linkEnabled=1 -head_plate_frameEnabled=1 -head_tilt_linkEnabled=1 -high_def_frameEnabled=1 -high_def_optical_frameEnabled=1 -imu_linkEnabled=1 -kinect_linkEnabled=1 -l_elbow_flex_linkEnabled=1 -l_forearm_cam_frameEnabled=1 -l_forearm_cam_optical_frameEnabled=1 -l_forearm_linkEnabled=1 -l_forearm_roll_linkEnabled=1 -l_gripper_l_finger_linkEnabled=1 -l_gripper_l_finger_tip_frameEnabled=1 -l_gripper_l_finger_tip_linkEnabled=1 -l_gripper_led_frameEnabled=1 -l_gripper_motor_accelerometer_linkEnabled=1 -l_gripper_motor_screw_linkEnabled=1 -l_gripper_motor_slider_linkEnabled=1 -l_gripper_palm_linkEnabled=1 -l_gripper_r_finger_linkEnabled=1 -l_gripper_r_finger_tip_linkEnabled=1 -l_gripper_tool_frameEnabled=1 -l_shoulder_lift_linkEnabled=1 -l_shoulder_pan_linkEnabled=1 -l_torso_lift_side_plate_linkEnabled=1 -l_upper_arm_linkEnabled=1 -l_upper_arm_roll_linkEnabled=1 -l_wrist_flex_linkEnabled=1 -l_wrist_roll_linkEnabled=1 -laser_tilt_linkEnabled=1 -laser_tilt_mount_linkEnabled=1 -mapEnabled=1 -narrow_stereo_gazebo_l_stereo_camera_frameEnabled=1 -narrow_stereo_gazebo_l_stereo_camera_optical_frameEnabled=1 -narrow_stereo_gazebo_r_stereo_camera_frameEnabled=1 -narrow_stereo_gazebo_r_stereo_camera_optical_frameEnabled=1 -narrow_stereo_linkEnabled=1 -narrow_stereo_optical_frameEnabled=1 -odom_combinedEnabled=1 -openni_cameraEnabled=1 -openni_depth_frameEnabled=1 -openni_depth_optical_frameEnabled=1 -openni_rgb_frameEnabled=1 -openni_rgb_optical_frameEnabled=1 -projector_wg6802418_child_frameEnabled=1 -projector_wg6802418_frameEnabled=1 -r_elbow_flex_linkEnabled=1 -r_forearm_cam_frameEnabled=1 -r_forearm_cam_optical_frameEnabled=1 -r_forearm_linkEnabled=1 -r_forearm_roll_linkEnabled=1 -r_gripper_l_finger_linkEnabled=1 -r_gripper_l_finger_tip_frameEnabled=1 -r_gripper_l_finger_tip_linkEnabled=1 -r_gripper_led_frameEnabled=1 -r_gripper_motor_accelerometer_linkEnabled=1 -r_gripper_motor_screw_linkEnabled=1 -r_gripper_motor_slider_linkEnabled=1 -r_gripper_palm_linkEnabled=1 -r_gripper_r_finger_linkEnabled=1 -r_gripper_r_finger_tip_linkEnabled=1 -r_gripper_tool_frameEnabled=1 -r_shoulder_lift_linkEnabled=1 -r_shoulder_pan_linkEnabled=1 -r_torso_lift_side_plate_linkEnabled=1 -r_upper_arm_linkEnabled=1 -r_upper_arm_roll_linkEnabled=1 -r_wrist_flex_linkEnabled=1 -r_wrist_roll_linkEnabled=1 -sensor_mount_linkEnabled=1 -torso_lift_linkEnabled=1 -torso_lift_motor_screw_linkEnabled=1 -wide_stereo_gazebo_l_stereo_camera_frameEnabled=1 -wide_stereo_gazebo_l_stereo_camera_optical_frameEnabled=1 -wide_stereo_gazebo_r_stereo_camera_frameEnabled=1 -wide_stereo_gazebo_r_stereo_camera_optical_frameEnabled=1 -wide_stereo_linkEnabled=1 -wide_stereo_optical_frameEnabled=1 -worldEnabled=1 -[TF./eng2] -2fEnabled=1 -3fEnabled=1 -7fEnabled=1 -8fEnabled=1 -[TF./eng2/7f] -73B2Enabled=1 diff --git a/elevator_move_base_pr2/trained_model/.gitignore b/elevator_move_base_pr2/trained_model/.gitignore new file mode 100644 index 0000000000..d6b7ef32c8 --- /dev/null +++ b/elevator_move_base_pr2/trained_model/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/jsk_maps/src/eng2-scene.l b/jsk_maps/src/eng2-scene.l index b890aa94a0..6d8744ec6f 100644 --- a/jsk_maps/src/eng2-scene.l +++ b/jsk_maps/src/eng2-scene.l @@ -50,7 +50,7 @@ :pos ,(float-vector 3100.0 -31250.0 0.0) :name "/eng2/2f/elevator_call_panel-front") `(:rot #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)) - :pos ,(float-vector 2000.0 -33800.0 0.0) + :pos ,(float-vector 2000.0 -33950.0 0.0) :name "/eng2/2f/elevator_inside_panel-front") `(:rot #2f((0.0 -1.0 0.0) (1.0 0.0 0.0) (0.0 0.0 1.0)) :pos ,(float-vector 2000.0 -31000.0 0.0) @@ -61,10 +61,16 @@ :name "/eng2/2f/subway-register") `(:pos ,(float-vector 25000.0 -9400.0 0.0) :rpy #f(1.5706 0 0) :name "/eng2/2f/subway-lunchset") - `(:pos ,(float-vector 3200.0 -31950 920) :rpy #f(1.5706 0 0) + `(:pos ,(float-vector 3100.0 -31950 920) :rpy #f(1.5706 0 0) :name "/eng2/2f/elevator_call_panel") - `(:pos ,(float-vector 2900 -33850 1000) :rpy #f(3.1416 0 0) - :name "/eng2/2f/elevator_inside_panel")) + `(:pos ,(float-vector 2900 -34000 1020) :rpy #f(3.1416 0 0) + :name "/eng2/2f/elevator_inside_panel") + `(:pos ,(float-vector 9800 -27000 0) :rpy #f(0 0 0) + :name "/eng2/2f/forum-door-inside") + `(:pos ,(float-vector 11200 -27000 0) :rpy #f(3.1416 0 0) + :name "/eng2/2f/forum-door-outside") + `(:pos ,(float-vector 10500 -27000 1000) :rpy #f(0 0 0) + :name "/eng2/2f/forum-door-button")) (setq scene (instance nav-scene-model :init :name "/eng2/2f" :objects (append (list map) (send map :descendants)))) (send scene :map map) @@ -127,7 +133,8 @@ :pos ,(float-vector 3100.0 -31250.0 0.0) :name "/eng2/7f/elevator_call_panel-front") `(:rot #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)) - :pos ,(float-vector 2000.0 -33800.0 0.0) + ;; :pos ,(float-vector 2000.0 -33800.0 0.0) + :pos ,(float-vector 2000.0 -33950.0 0.0) :name "/eng2/7f/elevator_inside_panel-front") `(:rot #2f((0.0 -1.0 0.0) (1.0 0.0 0.0) (0.0 0.0 1.0)) :pos ,(float-vector 2000.0 -31000.0 0.0) @@ -252,9 +259,9 @@ ;; `(:rot #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)) ;; :pos ,(float-vector 4084 7095 0.0) ;; :name "/eng2/7f/room73B2-chair") - `(:pos ,(float-vector 3200 -31950 920) :rpy #f(1.5706 0 0) + `(:pos ,(float-vector 3100 -31950 920) :rpy #f(1.5706 0 0) :name "/eng2/7f/elevator_call_panel") - `(:pos ,(float-vector 2900 -33800 1000) :rpy #f(3.1416 0 0) + `(:pos ,(float-vector 2900 -34000 1020) :rpy #f(3.1416 0 0) :name "/eng2/7f/elevator_inside_panel")) (setq scene (instance nav-scene-model :init :name "/eng2/7f" :objects (append (list map) (send map :descendants)))) @@ -293,14 +300,14 @@ :pos ,(float-vector 3100.0 -31250.0 0.0) :name "/eng2/8f/elevator_call_panel-front") `(:rot #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)) - :pos ,(float-vector 2000.0 -33800.0 0.0) + :pos ,(float-vector 2000.0 -33950.0 0.0) :name "/eng2/8f/elevator_inside_panel-front") `(:rot #2f((0.0 -1.0 0.0) (1.0 0.0 0.0) (0.0 0.0 1.0)) :pos ,(float-vector 2000.0 -31000.0 0.0) :name "/eng2/8f/elevator-outside") - `(:pos ,(float-vector 3200 -31950 920) :rpy #f(1.5706 0 0) + `(:pos ,(float-vector 3100 -31950 920) :rpy #f(1.5706 0 0) :name "/eng2/8f/elevator_call_panel") - `(:pos ,(float-vector 2900 -33800 1000) :rpy #f(3.1416 0 0) + `(:pos ,(float-vector 2900 -34000 1020) :rpy #f(3.1416 0 0) :name "/eng2/8f/elevator_inside_panel")) (setq scene (instance nav-scene-model :init :name "/eng2/8f" :objects (append (list map) (send map :descendants)))) @@ -325,7 +332,7 @@ (dolist (map maps) (let ((elevator (make-cube 2000 1800 2600 :name (format nil "~a/elevator" (send map :name))))) (send elevator :move-to (send map :copy-worldcoords)) - (send elevator :translate #f(1700 -33900 1300)) + (send elevator :translate #f(1700 -33950 1300)) (send map :assoc elevator) (send elevator :set-color #f(1 0 0 0.4)) (push elevator objects)