├── .gitignore ├── CMakeLists.txt ├── README.md ├── applications ├── manipulation_worlds │ ├── CMakeLists.txt │ ├── Makefile │ ├── home_with_bedroom.dae │ ├── home_with_bedroom.stl │ ├── home_with_bedroom.wings │ ├── launch │ │ ├── add_cabinet.launch │ │ ├── add_candy.launch │ │ ├── add_furnitures.launch │ │ ├── add_small_shelf.launch │ │ ├── empty_world.launch │ │ ├── home.launch │ │ ├── pr2_candy_world.launch │ │ ├── pr2_home.launch │ │ ├── pr2_object_retrieve.launch │ │ ├── pr2_small_shelf_world.launch │ │ └── pr2_table_object.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── meshes │ │ ├── coke_can.dae │ │ ├── coke_can.jpg │ │ ├── cup.dae │ │ ├── cup.stl │ │ ├── make_ply.py │ │ ├── make_small_white_shelf.py │ │ ├── make_sushi_shelf.py │ │ ├── peroxide.dae │ │ ├── small_white_shelf.dae │ │ ├── small_white_shelf.ply │ │ ├── small_white_shelf.stl │ │ ├── suave.dae │ │ ├── sushi_shelf.dae │ │ ├── sushi_shelf.ply │ │ ├── trash_bin.dae │ │ └── trash_bin.stl │ ├── urdf │ │ ├── cabinet.urdf │ │ ├── cabinet_door.urdf │ │ ├── candy1.urdf │ │ ├── candy2.urdf │ │ ├── candy_container.urdf │ │ ├── coke_can.urdf │ │ ├── cup.urdf │ │ ├── peroxide.urdf │ │ ├── plug.urdf │ │ ├── shelf.urdf │ │ ├── small_trash_bin.urdf │ │ ├── small_white_shelf.urdf │ │ ├── suave.urdf │ │ ├── sushi_shelf.urdf │ │ ├── table.urdf.xacro │ │ └── trash_bin.urdf │ └── worlds │ │ ├── empty.world │ │ └── home.world ├── pr2_create_object_model │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── action │ │ └── ModelObjectInHand.action │ ├── cmake │ │ └── FindEigen.cmake │ ├── launch │ │ └── create_object_model_server.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── msg │ │ ├── .gitignore │ │ └── ObjectInHand.msg │ └── src │ │ ├── create_object_model_server.cpp │ │ ├── ping_create_object_model_server.cpp │ │ └── pr2_create_object_model │ │ └── __init__.py ├── pr2_interactive_manipulation │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── cfg │ │ └── pickup_config.cfg │ ├── docs │ │ ├── PickupConfig-usage.dox │ │ ├── PickupConfig.dox │ │ └── PickupConfig.wikidoc │ ├── include │ │ └── pr2_interactive_manipulation │ │ │ ├── graspable_object_handler.h │ │ │ ├── interactive_manipulation_backend.h │ │ │ └── interactive_marker_node.h │ ├── launch │ │ ├── camera_throttle.launch │ │ ├── joy.launch │ │ └── pr2_interactive_manipulation_robot.launch │ ├── mainpage.dox │ ├── manifest.xml │ └── src │ │ ├── graspable_object_handler.cpp │ │ ├── interactive_manipulation_backend.cpp │ │ ├── interactive_manipulation_backend_node.cpp │ │ ├── interactive_marker_node.cpp │ │ └── pr2_interactive_manipulation │ │ ├── __init__.py │ │ └── cfg │ │ ├── .gitignore │ │ └── __init__.py ├── pr2_interactive_manipulation_frontend │ ├── CMakeLists.txt │ ├── Makefile │ ├── config │ │ └── pr2_interactive_manipulation.rviz │ ├── icons │ │ ├── classes │ │ │ ├── LookAt.png │ │ │ ├── NavigateTo.svg │ │ │ └── SetGripper.png │ │ ├── facing.png │ │ ├── icon_bg.svg │ │ ├── kinect.png │ │ ├── left.png │ │ ├── overhead.png │ │ ├── right.png │ │ └── top.png │ ├── image_src │ │ ├── facing.png │ │ ├── kinect.png │ │ ├── left.png │ │ ├── overhead.png │ │ └── top.png │ ├── images │ │ ├── splash.png │ │ └── splash.xcf │ ├── include │ │ └── pr2_interactive_manipulation │ │ │ ├── advanced_options_dialog.h │ │ │ ├── arm_locations.h │ │ │ ├── awesome_camera_display.h │ │ │ ├── camera_focus_frontend.h │ │ │ ├── interactive_manipulation_frontend.h │ │ │ ├── interactive_manipulation_frontend_display.h │ │ │ ├── invisible_robot_display.h │ │ │ ├── point_head_camera_display.h │ │ │ ├── point_head_view_controller.h │ │ │ ├── point_tools.h │ │ │ ├── publish_click_camera_display.h │ │ │ └── publish_click_view_controller.h │ ├── launch │ │ ├── big-menus.style │ │ ├── pr2_interactive_manipulation_desktop.launch │ │ └── startRvizRemap.sh │ ├── mainpage.dox │ ├── manifest.xml │ ├── plugin_description.xml │ ├── scripts │ │ └── adjust_rviz_config.py │ └── src │ │ ├── advanced_options_dialog.cpp │ │ ├── advanced_options_dialog.ui │ │ ├── awesome_camera_display.cpp │ │ ├── camera_focus_frontend.cpp │ │ ├── grasp_caller.cpp │ │ ├── interactive_manipulation.ui │ │ ├── interactive_manipulation_frontend.cpp │ │ ├── interactive_manipulation_frontend_display.cpp │ │ ├── invisible_robot_display.cpp │ │ ├── point_head_camera_display.cpp │ │ ├── point_head_view_controller.cpp │ │ ├── point_tools.cpp │ │ ├── publish_click_camera_display.cpp │ │ └── publish_click_view_controller.cpp ├── pr2_interactive_object_detection │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── action │ │ └── UserCommand.action │ ├── include │ │ └── pr2_interactive_object_detection │ │ │ ├── msg_saver.h │ │ │ └── pr2_interactive_object_detection_backend.h │ ├── launch │ │ └── pr2_interactive_object_detection_robot.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── msg │ │ └── .gitignore │ └── src │ │ ├── pr2_interactive_object_detection │ │ └── __init__.py │ │ ├── pr2_interactive_object_detection_backend.cpp │ │ └── recognition_translator.cpp ├── pr2_interactive_object_detection_frontend │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ │ └── pr2_interactive_object_detection_frontend │ │ │ ├── interactive_object_detection_display.h │ │ │ └── interactive_object_detection_frame.h │ ├── launch │ │ └── pr2_interactive_object_detection_desktop.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── plugin_description.xml │ └── src │ │ ├── init.cpp │ │ ├── interactive_object_detection_display.cpp │ │ ├── interactive_object_detection_frame.cpp │ │ └── main_frame.ui ├── pr2_pick_and_place_demos │ ├── CMakeLists.txt │ ├── Makefile │ ├── launch │ │ ├── pick_and_place_demo.launch │ │ └── pick_and_place_keyboard_interface.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── nodes │ │ └── pick_and_place_keyboard_interface.cpp │ ├── scripts │ │ └── run_pick_and_place_demo.py │ ├── src │ │ └── pr2_pick_and_place_demos │ │ │ ├── __init__.py │ │ │ ├── pick_and_place_demo.py │ │ │ └── pick_and_place_manager.py │ └── test │ │ ├── simple_pick_and_place_example.py │ │ └── test_pr2_gripper_grasp_planner_cluster.py ├── pr2_tabletop_manipulation_launch │ ├── CMakeLists.txt │ ├── Makefile │ ├── launch │ │ └── pr2_tabletop_manipulation.launch │ ├── mainpage.dox │ └── manifest.xml └── tf_throttle │ ├── CMakeLists.txt │ ├── Makefile │ ├── launch │ ├── tf_republished.launch │ └── tf_throttled.launch │ ├── mainpage.dox │ ├── manifest.xml │ └── src │ ├── tf_republish.cpp │ └── tf_throttle.cpp ├── manipulation ├── pr2_gripper_grasp_controller │ ├── CMakeLists.txt │ ├── Makefile │ ├── launch │ │ ├── both_grippers_finger_sensor_grasp_posture_action.launch │ │ └── both_grippers_grasp_posture_action.launch │ ├── mainpage.dox │ ├── manifest.xml │ └── nodes │ │ └── pr2_gripper_grasp_controller.cpp ├── pr2_gripper_grasp_planner_cluster │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── config │ │ ├── pr2_gripper_model.yaml │ │ ├── robotiq_hand_virtual_gripper_model.yaml │ │ └── schunk_hand_virtual_gripper_model.yaml │ ├── launch │ │ ├── pr2_gripper_grasp_planner_cluster_server.launch │ │ ├── robotiq_hand_grasp_planner_cluster_server.launch │ │ └── schunk_hand_grasp_planner_cluster_server.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── scripts │ │ └── point_cluster_grasp_planner_server.py │ ├── src │ │ └── pr2_gripper_grasp_planner_cluster │ │ │ ├── __init__.py │ │ │ └── point_cluster_grasp_planner.py │ └── srv │ │ └── SetPointClusterGraspParams.srv ├── pr2_gripper_reactive_approach │ ├── CMakeLists.txt │ ├── Makefile │ ├── mainpage.dox │ ├── manifest.xml │ ├── scripts │ │ └── reactive_grasp_server.py │ ├── src │ │ └── pr2_gripper_reactive_approach │ │ │ ├── __init__.py │ │ │ ├── controller_manager.py │ │ │ ├── controller_params.py │ │ │ ├── hand_sensor_listeners.py │ │ │ ├── joint_states_listener.py │ │ │ ├── reactive_grasp.py │ │ │ └── sensor_info.py │ └── test │ │ └── test_pr2_gripper_reactive_approach_server.py ├── pr2_gripper_sensor_action │ ├── CMakeLists.txt │ ├── Makefile │ ├── launch │ │ └── pr2_gripper_sensor_actions.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── scripts │ │ └── record_r_acc.sh │ └── src │ │ ├── findContactActionTest.cpp │ │ ├── forceServoActionTest.cpp │ │ ├── grabActionTest.cpp │ │ ├── placeActionTest.cpp │ │ ├── positionActionTest.cpp │ │ ├── pr2_gripper_event_detector_action.cpp │ │ ├── pr2_gripper_findContact_action.cpp │ │ ├── pr2_gripper_forceServo_action.cpp │ │ ├── pr2_gripper_grab_action.cpp │ │ ├── pr2_gripper_release_action.cpp │ │ ├── pr2_gripper_sensor_action.cpp │ │ ├── pr2_gripper_slipServo_action.cpp │ │ └── slipServoActionTest.cpp ├── pr2_gripper_sensor_controller │ ├── CMakeLists.txt │ ├── Makefile │ ├── controller_plugins.xml │ ├── include │ │ └── pr2_gripper_sensor_controller │ │ │ ├── acceleration_observer.h │ │ │ ├── digitalFilter.h │ │ │ ├── gripper_controller.h │ │ │ ├── pr2_gripper_sensor_controller.h │ │ │ └── pressure_observer.h │ ├── launch │ │ ├── pr2_gripper_sensor_controller.launch │ │ ├── pr2_gripper_sensor_controller.yaml │ │ ├── pr2_gripper_sensor_controller_left.yaml │ │ └── pr2_gripper_sensor_controller_right.yaml │ ├── mainpage.dox │ ├── manifest.xml │ └── src │ │ ├── acceleration_observer.cpp │ │ ├── digitalFilter.cpp │ │ ├── pr2_gripper_sensor_controller.cpp │ │ └── pressure_observer.cpp ├── pr2_gripper_sensor_msgs │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── action │ │ ├── PR2GripperEventDetector.action │ │ ├── PR2GripperFindContact.action │ │ ├── PR2GripperForceServo.action │ │ ├── PR2GripperGrab.action │ │ ├── PR2GripperRelease.action │ │ └── PR2GripperSlipServo.action │ ├── mainpage.dox │ ├── manifest.xml │ ├── msg │ │ ├── .gitignore │ │ ├── PR2GripperEventDetectorCommand.msg │ │ ├── PR2GripperEventDetectorData.msg │ │ ├── PR2GripperFindContactCommand.msg │ │ ├── PR2GripperFindContactData.msg │ │ ├── PR2GripperForceServoCommand.msg │ │ ├── PR2GripperForceServoData.msg │ │ ├── PR2GripperGrabCommand.msg │ │ ├── PR2GripperGrabData.msg │ │ ├── PR2GripperPressureData.msg │ │ ├── PR2GripperReleaseCommand.msg │ │ ├── PR2GripperReleaseData.msg │ │ ├── PR2GripperSensorRTState.msg │ │ ├── PR2GripperSensorRawData.msg │ │ ├── PR2GripperSlipServoCommand.msg │ │ └── PR2GripperSlipServoData.msg │ └── src │ │ └── pr2_gripper_sensor_msgs │ │ └── __init__.py ├── pr2_interactive_gripper_pose_action │ ├── CMakeLists.txt │ ├── Makefile │ ├── launch │ │ ├── pr2_interactive_gripper_pose_action.launch │ │ └── pr2_interactive_nav_action.launch │ ├── mainpage.dox │ ├── manifest.xml │ └── src │ │ ├── pr2_interactive_gripper_pose_action.cpp │ │ └── pr2_interactive_nav_action.cpp ├── pr2_manipulation_controllers │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── cmake │ │ └── FindEigen.cmake │ ├── controller_plugins.xml │ ├── include │ │ └── pr2_manipulation_controllers │ │ │ └── cartesian_trajectory_controller.h │ ├── mainpage.dox │ ├── manifest.xml │ ├── msg │ │ ├── JTTaskControllerState.msg │ │ ├── JTTeleopControllerState.msg │ │ ├── JinvExperimentalControllerState.msg │ │ └── JinvTeleopControllerState.msg │ ├── scripts │ │ └── posture.py │ ├── src │ │ ├── cartesian_trajectory_controller.cpp │ │ ├── jinv_experimental_controller.cpp │ │ ├── jinv_teleop_controller.cpp │ │ ├── jt_task_controller.cpp │ │ └── pr2_manipulation_controllers │ │ │ └── __init__.py │ └── srv │ │ ├── CheckMoving.srv │ │ └── MoveToPose.srv ├── pr2_marker_control │ ├── CMakeLists.txt │ ├── Makefile │ ├── TODO │ ├── include │ │ └── pr2_marker_control │ │ │ ├── cloud_handler.h │ │ │ ├── generate_robot_model.h │ │ │ └── marker_control.h │ ├── launch │ │ ├── desktop.launch │ │ └── marker_control.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── rviz_marker_control.vcg │ ├── scripts │ │ └── draw_reachable_zones.py │ └── src │ │ ├── cloud_handler.cpp │ │ ├── generate_robot_model.cpp │ │ ├── marker_control.cpp │ │ └── marker_control_node.cpp ├── pr2_object_manipulation_launch │ ├── CMakeLists.txt │ ├── Makefile │ ├── config │ │ ├── camera_configurations.yaml │ │ ├── collision_map_sources_laser.yaml │ │ ├── lcg_left_pr2_hand_descriptions.yaml │ │ ├── object_modeling_self_filter.yaml │ │ ├── pr2_arm_configurations.yaml │ │ ├── pr2_hand_descriptions.yaml │ │ └── self_filter.yaml │ ├── launch │ │ ├── c_jexp.launch │ │ ├── c_jinv.launch │ │ ├── c_jt.launch │ │ ├── c_jtask.launch │ │ ├── c_rmrc.launch │ │ ├── laser+stereo-perception.launch │ │ ├── pr2_manipulation.launch │ │ └── pr2_manipulation_prerequisites.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── manifest.xml~ │ └── scripts │ │ ├── change_joint_controller_gains.py │ │ └── kinect_monitor.py ├── pr2_object_manipulation_msgs │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── action │ │ ├── GetGripperPose.action │ │ ├── GetNavPose.action │ │ ├── GetPose.action │ │ ├── IMGUI.action │ │ ├── PickupIMObject.action │ │ ├── PoseStampedScripted.action │ │ ├── RunScript.action │ │ └── TestGripperPose.action │ ├── mainpage.dox │ ├── manifest.xml │ ├── msg │ │ ├── .gitignore │ │ ├── CameraFocus.msg │ │ ├── IMGUIAdvancedOptions.msg │ │ ├── IMGUICommand.msg │ │ ├── IMGUIOptions.msg │ │ ├── ImageClick.msg │ │ └── Ray.msg │ ├── src │ │ └── pr2_object_manipulation_msgs │ │ │ └── __init__.py │ └── srv │ │ └── ActionInfo.srv ├── pr2_wrappers │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ │ └── pr2_wrappers │ │ │ ├── base_client.h │ │ │ ├── gripper_controller.h │ │ │ ├── plugs_client.h │ │ │ ├── torso_client.h │ │ │ └── tuck_arms_client.h │ ├── manifest.xml │ └── src │ │ ├── base_client.cpp │ │ ├── gripper_controller.cpp │ │ ├── plugs_client.cpp │ │ ├── torso_client.cpp │ │ └── tuck_arms_client.cpp ├── rgbd_assembler │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ │ └── rgbd_assembler │ │ │ ├── msg_saver.h │ │ │ └── utils.h │ ├── launch │ │ ├── openni_node.launch │ │ ├── openni_node_new.launch │ │ ├── rgbd_assembler.launch │ │ ├── rgbd_kinect_assembler.launch │ │ └── sim_rgbd_assembler.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── src │ │ ├── ping_rgbd_assembler_node.cpp │ │ ├── rgbd_assembler │ │ │ └── __init__.py │ │ ├── rgbd_assembler_kinect_node.cpp │ │ └── rgbd_assembler_node.cpp │ └── srv │ │ └── RgbdAssembly.srv └── segmented_clutter_grasp_planner │ ├── CMakeLists.txt │ ├── Makefile │ ├── launch │ └── segmented_clutter_grasp_planner_server.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── scripts │ └── segmented_clutter_grasp_planner_server.py │ ├── src │ ├── find_container_action.cpp │ ├── segmented_clutter_grasp_planner │ │ ├── __init__.py │ │ └── segmented_clutter_grasp_planner.py │ ├── test_find_container_action.cpp │ └── write_transformed_pcd.cpp │ └── test │ └── test_segmented_clutter_grasp_planner_server.py ├── navigation └── pr2_navigation_controllers │ ├── CMakeLists.txt │ ├── CMakeLists.txt.user │ ├── Makefile │ ├── include │ └── pr2_navigation_controllers │ │ ├── goal_passer.h │ │ └── pose_follower.h │ ├── manifest.xml │ ├── planner_plugins.xml │ └── src │ ├── goal_passer.cpp │ └── pose_follower.cpp ├── perception ├── interactive_perception_msgs │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── action │ │ ├── ObjectRecognitionGui.action │ │ └── ObjectSegmentationGui.action │ ├── mainpage.dox │ ├── manifest.xml │ └── msg │ │ ├── .gitignore │ │ ├── ModelHypothesis.msg │ │ └── ModelHypothesisList.msg ├── object_recognition_gui │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ │ └── object_recognition_gui │ │ │ ├── mouse_event_signalling_render_panel.h │ │ │ ├── object_recognition_display.h │ │ │ └── object_recognition_rviz_ui.h │ ├── lib │ │ └── rviz_plugin.yaml │ ├── mainpage.dox │ ├── manifest.xml │ ├── plugin_description.xml │ └── src │ │ ├── init.cpp │ │ ├── mouse_event_signalling_render_panel.cpp │ │ ├── object_recognition_display.cpp │ │ ├── object_recognition_frame.ui │ │ ├── object_recognition_gui │ │ └── __init__.py │ │ ├── object_recognition_rviz_ui.cpp │ │ └── ping_object_recognition_gui.cpp ├── robot_self_filter_color │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ │ └── robot_self_filter_color │ │ │ ├── self_mask_color.h │ │ │ └── self_see_filter_color.h │ ├── mainpage.dox │ ├── manifest.xml │ └── src │ │ ├── self_filter_color.cpp │ │ ├── self_mask_color.cpp │ │ └── test_filter_color.cpp ├── tabletop_collision_map_processing │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ │ └── tabletop_collision_map_processing │ │ │ └── collision_map_interface.h │ ├── mainpage.dox │ ├── manifest.xml │ ├── nodes │ │ └── tabletop_collision_map_processing_node.cpp │ ├── src │ │ ├── collision_map_interface.cpp │ │ └── tabletop_collision_map_processing │ │ │ ├── __init__.py │ │ │ └── collision_map_interface.py │ └── srv │ │ └── TabletopCollisionMapProcessing.srv └── tabletop_object_detector │ ├── .gitignore │ ├── CMakeLists.txt │ ├── Makefile │ ├── include │ └── tabletop_object_detector │ │ ├── exhaustive_fit_detector.h │ │ ├── icp_registration_fitter.h │ │ ├── iterative_distance_fitter.h │ │ ├── marker_generator.h │ │ └── model_fitter.h │ ├── launch │ ├── tabletop_complete.launch │ ├── tabletop_complete_standalone.launch │ ├── tabletop_object_recognition.launch │ └── tabletop_segmentation.launch │ ├── mainpage.dox │ ├── manifest.xml │ ├── msg │ ├── Table.msg │ └── TabletopDetectionResult.msg │ ├── src │ ├── iterative_distance_fitter.cpp │ ├── marker_generator.cpp │ ├── model_fitter.cpp │ ├── ping_segment_object_in_hand.cpp │ ├── ping_tabletop_node.cpp │ ├── publish_database_object.cpp │ ├── segment_object_in_hand.cpp │ ├── tabletop_complete_node.cpp │ ├── tabletop_object_detector │ │ └── __init__.py │ ├── tabletop_object_recognition.cpp │ └── tabletop_segmentation.cpp │ └── srv │ ├── AddModelExclusion.srv │ ├── ClearExclusionsList.srv │ ├── NegateExclusions.srv │ ├── SegmentObjectInHand.srv │ ├── TabletopDetection.srv │ ├── TabletopObjectRecognition.srv │ └── TabletopSegmentation.srv ├── rosdep.yaml └── stack.xml /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | bin 3 | lib 4 | *.pyc 5 | *~ 6 | srv_gen 7 | msg_gen 8 | cfg/cpp 9 | *.cfgc -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Append to CPACK_SOURCE_IGNORE_FILES a semicolon-separated list of 5 | # directories (or patterns, but directories should suffice) that should 6 | # be excluded from the distro. This is not the place to put things that 7 | # should be ignored everywhere, like "build" directories; that happens in 8 | # rosbuild/rosbuild.cmake. Here should be listed packages that aren't 9 | # ready for inclusion in a distro. 10 | # 11 | # This list is combined with the list in rosbuild/rosbuild.cmake. Note 12 | # that CMake 2.6 may be required to ensure that the two lists are combined 13 | # properly. CMake 2.4 seems to have unpredictable scoping rules for such 14 | # variables. 15 | #list(APPEND CPACK_SOURCE_IGNORE_FILES /core/experimental) 16 | 17 | rosbuild_make_distribution(0.7.5) 18 | 19 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | pr2_object_manipulation 2 | ======================= 3 | 4 | PR2-specific functionality related to pickup and place tasks. -------------------------------------------------------------------------------- /applications/manipulation_worlds/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/manipulation_worlds/home_with_bedroom.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/manipulation_worlds/home_with_bedroom.stl -------------------------------------------------------------------------------- /applications/manipulation_worlds/home_with_bedroom.wings: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/manipulation_worlds/home_with_bedroom.wings -------------------------------------------------------------------------------- /applications/manipulation_worlds/launch/add_cabinet.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/launch/empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/launch/home.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/launch/pr2_candy_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/launch/pr2_home.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/launch/pr2_small_shelf_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b manipulation_worlds is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | manipulation_worlds 5 | 6 | 7 | Matei Ciocarlie 8 | BSD 9 | 10 | http://ros.org/wiki/manipulation_worlds 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/meshes/coke_can.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/manipulation_worlds/meshes/coke_can.jpg -------------------------------------------------------------------------------- /applications/manipulation_worlds/meshes/cup.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/manipulation_worlds/meshes/cup.stl -------------------------------------------------------------------------------- /applications/manipulation_worlds/meshes/make_small_white_shelf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from make_ply import * 3 | 4 | outfile = file("small_white_shelf.ply", 'w') 5 | 6 | vertices = [] 7 | faces = [] 8 | add_box([0.,0.,0.], 31.4, 29.5, 1.2, vertices, faces) 9 | add_box([0.,0.,30.], 31.4, 29.5, 1.2, vertices, faces) 10 | add_box([0.,0.,60.], 31.4, 29.5, 1.2, vertices, faces) 11 | add_box([0.,0.,1.2], 1.4, 29.5, 58.8, vertices, faces) 12 | add_box([30.,0.,1.2], 1.4, 29.5, 58.8, vertices, faces) 13 | add_box([0.,29.25,1.2], 31.4, 0.25, 58.8, vertices, faces) 14 | 15 | #convert from cm to m 16 | for i in range(len(vertices)): 17 | for j in range(3): 18 | vertices[i][j] *= .01 19 | 20 | write_ply(vertices, faces, outfile) 21 | 22 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/meshes/make_sushi_shelf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from make_ply import * 3 | 4 | outfile = file("sushi_shelf.ply", 'w') 5 | 6 | vertices = [] 7 | faces = [] 8 | #vertical posts 9 | add_box([0.,0.,0.], 8.5,1.5,174., vertices, faces) 10 | add_box([73.4,0.,0.], 8.5,1.5,174., vertices, faces) 11 | add_box([146.8,0.,0.], 8.5,1.5,174., vertices, faces) 12 | add_box([0.,31.8,0.], 8.5,1.5,174., vertices, faces) 13 | add_box([73.4,31.8,0], 8.5,1.5,174., vertices, faces) 14 | add_box([146.8,31.8,0.], 8.5,1.5,174., vertices, faces) 15 | 16 | #shelves 17 | add_box([0.,2.4,4.6], 155.3,28.5,1.5, vertices, faces) 18 | add_box([0.,2.4,55.8], 155.3,28.5,1.5, vertices, faces) 19 | add_box([0.,2.4,94.2], 155.3,28.5,1.5, vertices, faces) 20 | add_box([0.,2.4,171.], 155.3,28.5,1.5, vertices, faces) 21 | 22 | #convert from cm to m 23 | for i in range(len(vertices)): 24 | for j in range(3): 25 | vertices[i][j] *= .01 26 | 27 | write_ply(vertices, faces, outfile) 28 | 29 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/meshes/small_white_shelf.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/manipulation_worlds/meshes/small_white_shelf.stl -------------------------------------------------------------------------------- /applications/manipulation_worlds/meshes/trash_bin.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/manipulation_worlds/meshes/trash_bin.stl -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/candy1.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Gazebo/Red 26 | false 27 | true 28 | 100000000.0 29 | 1000000.0 30 | 31 | true 32 | 1000 33 | 10 34 | candy1_grasp_hack 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/candy2.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | Gazebo/Green 27 | false 28 | true 29 | 100000000.0 30 | 1000000.0 31 | 32 | true 33 | 1000 34 | 10 35 | candy1_grasp_hack 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/candy_container.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Gazebo/Blue 26 | false 27 | true 28 | 100000000.0 29 | 1000000.0 30 | 31 | 32 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/coke_can.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Gazebo/Red 26 | false 27 | 28 | 29 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/cup.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | false 26 | true 27 | 100000000.0 28 | 1000000.0 29 | 30 | true 31 | 1000 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/peroxide.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Gazebo/Yellow 26 | false 27 | true 28 | 100000000.0 29 | 5000000.0 30 | 31 | true 32 | 1000 33 | 10 34 | peroxide_grasp_hack 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/shelf.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | false 26 | true 27 | 100000000.0 28 | 10000.0 29 | 30 | 31 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/small_trash_bin.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | false 26 | true 27 | 100000000.0 28 | 1000000.0 29 | 30 | 31 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/small_white_shelf.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Gazebo/White 26 | false 27 | true 28 | 100000000.0 29 | 1000000.0 30 | 0.3 31 | 0.3 32 | 33 | 34 | true 35 | 36 | 37 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/suave.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Gazebo/Green 26 | false 27 | true 28 | 100000000.0 29 | 5000000.0 30 | 31 | true 32 | 1000 33 | 10 34 | suave_grasp_hack 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/sushi_shelf.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | Gazebo/Yellow 26 | false 27 | true 28 | 100000000.0 29 | 1000000.0 30 | 31 | 32 | -------------------------------------------------------------------------------- /applications/manipulation_worlds/urdf/trash_bin.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | false 26 | true 27 | 100000000.0 28 | 1000000.0 29 | 30 | 31 | -------------------------------------------------------------------------------- /applications/pr2_create_object_model/.gitignore: -------------------------------------------------------------------------------- 1 | src/pr2_create_object_model/msg/ -------------------------------------------------------------------------------- /applications/pr2_create_object_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 5 | find_package(Eigen REQUIRED) 6 | include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) 7 | 8 | 9 | # Set the build type. Options are: 10 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 11 | # Debug : w/ debug symbols, w/o optimization 12 | # Release : w/o debug symbols, w/ optimization 13 | # RelWithDebInfo : w/ debug symbols, w/ optimization 14 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 15 | #set(ROS_BUILD_TYPE RelWithDebInfo) 16 | 17 | #generate the actions 18 | rosbuild_find_ros_package(actionlib_msgs) 19 | include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 20 | genaction() 21 | 22 | rosbuild_init() 23 | 24 | #set the default path for built executables to the "bin" directory 25 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 26 | #set the default path for built libraries to the "lib" directory 27 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 28 | 29 | #uncomment if you have defined messages 30 | rosbuild_genmsg() 31 | #uncomment if you have defined services 32 | #rosbuild_gensrv() 33 | 34 | #common commands for building c++ executables and libraries 35 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 36 | #target_link_libraries(${PROJECT_NAME} another_library) 37 | #rosbuild_add_boost_directories() 38 | #rosbuild_link_boost(${PROJECT_NAME} thread) 39 | #rosbuild_add_executable(example examples/example.cpp) 40 | #target_link_libraries(example ${PROJECT_NAME}) 41 | 42 | add_definitions(${EIGEN_DEFINITIONS}) 43 | 44 | rosbuild_add_executable(create_object_model_server src/create_object_model_server.cpp) 45 | rosbuild_add_executable(ping_create_object_model_server src/ping_create_object_model_server.cpp) 46 | -------------------------------------------------------------------------------- /applications/pr2_create_object_model/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/pr2_create_object_model/action/ModelObjectInHand.action: -------------------------------------------------------------------------------- 1 | # which arm to use 2 | string arm_name 3 | 4 | # the relative motion in which to move the gripper to get it clear of objects 5 | #(if left unfilled, will not move) 6 | geometry_msgs/Vector3Stamped clear_move 7 | 8 | # the pose to go to for rotating (if left unfilled, will not go) 9 | geometry_msgs/PoseStamped rotate_pose 10 | 11 | # whether to rotate the object at rotate_pose to build up the model 12 | uint8 rotate_object 13 | 14 | # whether to add the object to the collision map 15 | uint8 add_to_collision_map 16 | 17 | # whether to keep the object level while rotating 18 | uint8 keep_level 19 | 20 | --- 21 | 22 | # the resulting object point cloud 23 | sensor_msgs/PointCloud2 cluster 24 | 25 | # the resulting collision name, if added to the collision map 26 | string collision_name 27 | 28 | --- 29 | 30 | # which phase the process is in 31 | int32 phase 32 | int32 BEFORE_MOVE=0 33 | int32 CLEAR_MOVE=1 34 | int32 MOVE_TO_ROTATE_POSE=2 35 | int32 ROTATING=3 36 | int32 DONE=4 37 | 38 | # how many rotate-poses have we gone to/are we in now 39 | int32 rotate_ind 40 | -------------------------------------------------------------------------------- /applications/pr2_create_object_model/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_create_object_model is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /applications/pr2_create_object_model/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pr2_create_object_model 5 | 6 | 7 | Kaijen Hsiao 8 | BSD 9 | 10 | http://ros.org/wiki/pr2_create_object_model 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /applications/pr2_create_object_model/msg/.gitignore: -------------------------------------------------------------------------------- 1 | ModelObjectInHandActionFeedback.msg 2 | ModelObjectInHandActionGoal.msg 3 | ModelObjectInHandAction.msg 4 | ModelObjectInHandActionResult.msg 5 | ModelObjectInHandFeedback.msg 6 | ModelObjectInHandGoal.msg 7 | ModelObjectInHandResult.msg 8 | -------------------------------------------------------------------------------- /applications/pr2_create_object_model/msg/ObjectInHand.msg: -------------------------------------------------------------------------------- 1 | string arm_name 2 | sensor_msgs/PointCloud2 cluster 3 | string collision_name 4 | -------------------------------------------------------------------------------- /applications/pr2_create_object_model/src/pr2_create_object_model/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_create_object_model/src/pr2_create_object_model/__init__.py -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/.gitignore: -------------------------------------------------------------------------------- 1 | cfg/cpp -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 3 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 4 | 5 | # CMake 2.4.6 doesn't include FindPkgConfig, so we provide our own copy 6 | include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) 7 | 8 | find_package(Eigen REQUIRED) 9 | include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) 10 | 11 | # Set the build type. Options are: 12 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 13 | # Debug : w/ debug symbols, w/o optimization 14 | # Release : w/o debug symbols, w/ optimization 15 | # RelWithDebInfo : w/ debug symbols, w/ optimization 16 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 17 | set(ROS_BUILD_TYPE RelWithDebInfo) 18 | 19 | rosbuild_init() 20 | 21 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 22 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 23 | 24 | # Dynamic reconfigure 25 | rosbuild_find_ros_package(dynamic_reconfigure) 26 | include(${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake) 27 | gencfg() 28 | 29 | 30 | # backend node 31 | rosbuild_add_executable(interactive_manipulation_backend_node 32 | src/interactive_manipulation_backend.cpp 33 | src/interactive_manipulation_backend_node.cpp 34 | ) 35 | 36 | # user interface based on interactive markers 37 | rosbuild_add_executable(interactive_marker_node 38 | src/interactive_marker_node.cpp 39 | src/graspable_object_handler.cpp 40 | ) 41 | 42 | rosbuild_add_roslaunch_check( launch/joy.launch) 43 | rosbuild_add_roslaunch_check( launch/pr2_interactive_manipulation_robot.launch ROBOT=sim) 44 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/cfg/pickup_config.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | PACKAGE='pr2_interactive_manipulation' 4 | import roslib; roslib.load_manifest(PACKAGE) 5 | 6 | from dynamic_reconfigure.parameter_generator import * 7 | 8 | gen = ParameterGenerator() 9 | 10 | 11 | gen.add("reactive_grasping", bool_t, 1, "Reactive Grasping", False) 12 | gen.add("lift_steps", int_t, 1, "Lift After Grasp (cm)", 10, 0, 10) 13 | gen.add("lift_vertically", bool_t, 1, "Lift Vertically", True) 14 | 15 | exit(gen.generate(PACKAGE, "interactive_manipulation_marker_node", "Pickup")) 16 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/docs/PickupConfig-usage.dox: -------------------------------------------------------------------------------- 1 | \subsubsection usage Usage 2 | \verbatim 3 | 4 | 5 | 6 | 7 | 8 | \endverbatim 9 | 10 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/docs/PickupConfig.dox: -------------------------------------------------------------------------------- 1 | \subsubsection parameters ROS parameters 2 | 3 | Reads and maintains the following parameters on the ROS server 4 | 5 | - \b "~reactive_grasping" : \b [bool] Reactive Grasping min: False, default: False, max: True 6 | - \b "~lift_steps" : \b [int] Lift After Grasp (cm) min: 0, default: 10, max: 10 7 | - \b "~lift_vertically" : \b [bool] Lift Vertically min: False, default: True, max: True 8 | 9 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/docs/PickupConfig.wikidoc: -------------------------------------------------------------------------------- 1 | # Autogenerated param section. Do not hand edit. 2 | param { 3 | group.0 { 4 | name=Dynamically Reconfigurable Parameters 5 | desc=See the [[dynamic_reconfigure]] package for details on dynamically reconfigurable parameters. 6 | 0.name= ~reactive_grasping 7 | 0.default= False 8 | 0.type= bool 9 | 0.desc=Reactive Grasping 10 | 1.name= ~lift_steps 11 | 1.default= 10 12 | 1.type= int 13 | 1.desc=Lift After Grasp (cm) Range: 0 to 10 14 | 2.name= ~lift_vertically 15 | 2.default= True 16 | 2.type= bool 17 | 2.desc=Lift Vertically 18 | } 19 | } 20 | # End of autogenerated section. You may edit below. 21 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/launch/camera_throttle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 13 | 14 | 17 | 18 | 19 | 22 | 23 | 24 | 27 | 28 | 29 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/launch/joy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | autorepeat_rate: 10 6 | deadzone: 0.12 7 | dev: "/dev/input/js0" 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_interactive_manipulation is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/src/pr2_interactive_manipulation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation/src/pr2_interactive_manipulation/__init__.py -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/src/pr2_interactive_manipulation/cfg/.gitignore: -------------------------------------------------------------------------------- 1 | PickupConfig.py 2 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation/src/pr2_interactive_manipulation/cfg/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation/src/pr2_interactive_manipulation/cfg/__init__.py -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/classes/LookAt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/classes/LookAt.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/classes/SetGripper.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/classes/SetGripper.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/facing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/facing.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/kinect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/kinect.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/left.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/overhead.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/overhead.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/right.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/icons/top.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/icons/top.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/image_src/facing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/image_src/facing.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/image_src/kinect.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/image_src/kinect.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/image_src/left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/image_src/left.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/image_src/overhead.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/image_src/overhead.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/image_src/top.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/image_src/top.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/images/splash.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/images/splash.png -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/images/splash.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_interactive_manipulation_frontend/images/splash.xcf -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/launch/big-menus.style: -------------------------------------------------------------------------------- 1 | QMenu::item { 2 | padding: 20px; 3 | color: black; 4 | background-color: white; 5 | } 6 | 7 | QMenu::item:selected { 8 | background: #CCDDFF; 9 | } 10 | 11 | * { 12 | font: 22px; 13 | } 14 | 15 | QPushButton { 16 | padding: 20px; 17 | margin: 20px; 18 | min-width: 32px 19 | min-height: 32px 20 | } 21 | 22 | QToolButton { 23 | padding: 20px; 24 | margin: 20px; 25 | min-width: 32px 26 | min-height: 32px 27 | } 28 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/launch/startRvizRemap.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #for use with glc-capture 3 | /opt/ros/unstable/stacks/visualization/rviz/bin/rviz right_arm/constraint_aware_ik:=/pr2_right_arm_kinematics/get_constraint_aware_ik left_arm/constraint_aware_ik:=/pr2_left_arm_kinematics/get_constraint_aware_ik right_arm/get_ik_solver_info:=/pr2_right_arm_kinematics/get_ik_solver_info left_arm/get_ik_solver_info:=/pr2_left_arm_kinematics/get_ik_solver_info right_arm/get_state_validity:=/environment_server_right_arm/get_state_validity left_arm/get_state_validity:=/environment_server_left_arm/get_state_validity right_arm/move_arm:=/move_right_arm left_arm/move_arm:=/move_left_arm right_arm/hand_posture_execution:=/r_gripper_grasp_posture_controller left_arm/hand_posture_execution:=/l_gripper_grasp_posture_controller right_arm/grasp_status:=/r_gripper_grasp_status left_arm/grasp_status:=/l_gripper_grasp_status right_arm/joint_trajectory:=/r_arm_controller/joint_trajectory_action left_arm/joint_trajectory:=/l_arm_controller/joint_trajectory_action -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_interactive_manipulation_frontend 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /applications/pr2_interactive_manipulation_frontend/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pr2_interactive_manipulation_frontend 5 | 6 | 7 | Jonathan Binney 8 | BSD 9 | 10 | http://ros.org/wiki/pr2_interactive_manipulation_frontend 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/.gitignore: -------------------------------------------------------------------------------- 1 | src/pr2_interactive_object_detection/msg/ 2 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | # CMake 2.4.6 doesn't include FindPkgConfig, so we provide our own copy 4 | include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) 5 | 6 | 7 | # Set the build type. Options are: 8 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 9 | # Debug : w/ debug symbols, w/o optimization 10 | # Release : w/o debug symbols, w/ optimization 11 | # RelWithDebInfo : w/ debug symbols, w/ optimization 12 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 13 | #set(ROS_BUILD_TYPE RelWithDebInfo) 14 | 15 | #generate the actions 16 | rosbuild_find_ros_package(actionlib_msgs) 17 | include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 18 | genaction() 19 | 20 | rosbuild_init() 21 | 22 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 23 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 24 | 25 | rosbuild_genmsg() 26 | 27 | ###backend node 28 | 29 | rosbuild_add_executable(pr2_interactive_object_detection_backend src/pr2_interactive_object_detection_backend.cpp) 30 | 31 | rosbuild_add_roslaunch_check(launch) 32 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/action/UserCommand.action: -------------------------------------------------------------------------------- 1 | #request constants 2 | int8 LOOK_AT_TABLE=0 3 | 4 | int8 SEGMENT=1 5 | int8 RECOGNIZE=2 6 | int8 DETECT=3 7 | int8 RESET=4 8 | 9 | int8 request 10 | bool interactive 11 | --- 12 | #no specific result 13 | --- 14 | #feedback in the form of a status string 15 | string status 16 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_interactive_manipulation is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Backend for the interactive object recognition rviz plugin. 4 | 5 | David Gossow 6 | BSD 7 | 8 | http://ros.org/wiki/pr2_interactive_object_detection 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/msg/.gitignore: -------------------------------------------------------------------------------- 1 | UserCommandActionFeedback.msg 2 | UserCommandActionGoal.msg 3 | UserCommandAction.msg 4 | UserCommandActionResult.msg 5 | UserCommandFeedback.msg 6 | UserCommandGoal.msg 7 | UserCommandResult.msg 8 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection/src/pr2_interactive_object_detection/__init__.py: -------------------------------------------------------------------------------- 1 | #autogenerated by ROS python message generators -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection_frontend/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection_frontend/launch/pr2_interactive_object_detection_desktop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection_frontend/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_interactive_object_detection_frontend 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection_frontend/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | A package that allows a remote user to request and assist 6 | the detection, recognition and pose estimation of tabletop objects, 7 | primarily using an rviz display. 8 | 9 | 10 | 11 | jbinney 12 | BSD 13 | 14 | http://ros.org/wiki/pr2_interactive_object_detection_frontend 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /applications/pr2_interactive_object_detection_frontend/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Allows you to trigger the interactive segmentation 7 | of tabletop scenes as well as object recognition. 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /applications/pr2_pick_and_place_demos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #rosbuild_find_ros_package(actionlib_msgs) 13 | #include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 14 | #genaction() 15 | 16 | rosbuild_init() 17 | 18 | #set the default path for built executables to the "bin" directory 19 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 20 | #set the default path for built libraries to the "lib" directory 21 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 22 | 23 | #uncomment if you have defined messages 24 | rosbuild_genmsg() 25 | #uncomment if you have defined services 26 | #rosbuild_gensrv() 27 | 28 | #common commands for building c++ executables and libraries 29 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 30 | #target_link_libraries(${PROJECT_NAME} another_library) 31 | #rosbuild_add_boost_directories() 32 | #rosbuild_link_boost(${PROJECT_NAME} thread) 33 | #rosbuild_add_executable(example examples/example.cpp) 34 | #target_link_libraries(example ${PROJECT_NAME}) 35 | 36 | #rosbuild_add_executable(pick_and_place_keyboard_interface nodes/pick_and_place_keyboard_interface.cpp) 37 | -------------------------------------------------------------------------------- /applications/pr2_pick_and_place_demos/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/pr2_pick_and_place_demos/launch/pick_and_place_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /applications/pr2_pick_and_place_demos/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_pick_and_place_demos is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /applications/pr2_pick_and_place_demos/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Example applications that call the services provided by the 5 | manipulation pipeline. 6 | 7 | 8 | Matei Ciocarlie and Kaijen Hsiao 9 | BSD 10 | 11 | http://ros.org/wiki/pr2_pick_and_place_demos 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /applications/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/applications/pr2_pick_and_place_demos/src/pr2_pick_and_place_demos/__init__.py -------------------------------------------------------------------------------- /applications/pr2_tabletop_manipulation_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | -------------------------------------------------------------------------------- /applications/pr2_tabletop_manipulation_launch/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/pr2_tabletop_manipulation_launch/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_tabletop_manipulation_launch is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /applications/pr2_tabletop_manipulation_launch/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Launch files for starting the manipulation pipeline, complete with 5 | sensor processing for manipulation, on the PR2 robot. 6 | 7 | 8 | Matei Ciocarlie 9 | BSD 10 | 11 | http://ros.org/wiki/pr2_tabletop_manipulation_launch 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /applications/tf_throttle/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | rosbuild_add_executable(tf_throttle src/tf_throttle.cpp) 33 | 34 | rosbuild_add_executable(tf_republish src/tf_republish.cpp) 35 | -------------------------------------------------------------------------------- /applications/tf_throttle/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /applications/tf_throttle/launch/tf_republished.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /applications/tf_throttle/launch/tf_throttled.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /applications/tf_throttle/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b tf_throttle 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /applications/tf_throttle/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | tf_throttle 5 | 6 | 7 | Matei Ciocarlie 8 | BSD 9 | 10 | http://ros.org/wiki/tf_throttle 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | rosbuild_add_executable(pr2_gripper_grasp_controller nodes/pr2_gripper_grasp_controller.cpp) 33 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_controller/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_controller/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_grasp_controller is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_controller/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Implementation of the hand posture grasp controller for the PR2 5 | gripper. 6 | 7 | 8 | Matei Ciocarlie 9 | BSD 10 | 11 | http://ros.org/wiki/pr2_gripper_grasp_controller 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/.gitignore: -------------------------------------------------------------------------------- 1 | src/pr2_gripper_grasp_planner_cluster/srv/ 2 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/launch/pr2_gripper_grasp_planner_cluster_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/launch/robotiq_hand_grasp_planner_cluster_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/launch/schunk_hand_grasp_planner_cluster_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_grasp_planner_cluster is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Plans grasps for unknown objects using the PR2 gripper. Uses only 5 | a (segmented) point cloud of the target object, often originating 6 | from a single view or scan. 7 | 8 | 9 | Kaijen Hsiao 10 | BSD 11 | 12 | http://ros.org/wiki/pr2_gripper_grasp_planner_cluster 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/src/pr2_gripper_grasp_planner_cluster/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_grasp_planner_cluster/srv/SetPointClusterGraspParams.srv: -------------------------------------------------------------------------------- 1 | #only want side grasps if the bounding box height is greater than this 2 | float64 height_good_for_side_grasps 3 | 4 | #bounding box "fits in hand" if the relevant dimension is less than this 5 | float64 gripper_opening 6 | 7 | #how far to move the gripper to the side with each step when searching for grasps 8 | float64 side_step 9 | 10 | #how far to move the palm inward with each step when searching for grasps 11 | float64 palm_step 12 | 13 | #set this to true to limit the planner to overhead grasps 14 | bool overhead_grasps_only 15 | 16 | #set this to true to limit the planner to side grasps 17 | bool side_grasps_only 18 | 19 | #set this to false to omit random high-point grasps 20 | bool include_high_point_grasps 21 | 22 | #set this to true to make the pregrasps be just outside the bounding box instead of self.pregrasp_dist away from the grasp 23 | bool pregrasp_just_outside_box 24 | 25 | #how many backing-off-from-the-deepest-possible-grasp grasps to keep for each good grasp found 26 | int32 backoff_depth_steps 27 | 28 | #don't check the neighbors for each grasp (reduces grasps checked, but makes for worse rankings) 29 | bool disable_grasp_neighbor_check 30 | 31 | #set this to true to randomize the order of the first 30 grasps 32 | bool randomize_grasps 33 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_reactive_approach/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_reactive_approach/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_gripper_reactive_approach/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_reactive_approach is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_reactive_approach/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Executes a grasp with the PR2 gripper, starting from the 5 | pre-grasp stage and using the tactile sensors to correct for 6 | unexpected contacts encountered along the way. 7 | 8 | 9 | Kaijen Hsiao 10 | BSD 11 | 12 | http://ros.org/wiki/pr2_gripper_reactive_approach 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_reactive_approach/src/pr2_gripper_reactive_approach/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_action/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_action/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_sensor_action is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_action/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The pr2_gripper_sensor_action package provides an action interface for talking to the pr2_gripper_sensor_controller real-time controller. 5 | 6 | It provides several different actions for getting high-level sensor information from the PR2 palm-mounted accelerometers, fingertip pressure arrays, and gripper motor/encoder, as well as several sensor-based gripper control actions that respond with low-latency in real-time. 7 | 8 | 9 | Joe Romano 10 | BSD 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_action/scripts/record_r_acc.sh: -------------------------------------------------------------------------------- 1 | # record $2 samples to file defined by $1 2 | # example: record_r_acc my_filename 30000 3 | rostopic echo -p /r_gripper_sensor_controller/event_detector_state | head -n $2 > "$1.rtp" -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | 13 | 14 | 15 | rosbuild_init() 16 | 17 | #set the default path for built executables to the "bin" directory 18 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 19 | #set the default path for built libraries to the "lib" directory 20 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 21 | 22 | #uncomment if you have defined messages 23 | #rosbuild_genmsg() 24 | #uncomment if you have defined services 25 | #rosbuild_gensrv() 26 | 27 | #common commands for building c++ executables and libraries 28 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 29 | #target_link_libraries(${PROJECT_NAME} another_library) 30 | #rosbuild_add_boost_directories() 31 | #rosbuild_link_boost(${PROJECT_NAME} thread) 32 | #rosbuild_add_executable(example examples/example.cpp) 33 | #target_link_libraries(example ${PROJECT_NAME}) 34 | rosbuild_add_library(pr2_gripper_sensor_controller_lib src/digitalFilter.cpp 35 | src/pr2_gripper_sensor_controller.cpp 36 | src/pressure_observer.cpp 37 | src/acceleration_observer.cpp) 38 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/include/pr2_gripper_sensor_controller/digitalFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ---------------------------------------------------------------------------- 3 | * "THE BEER-WARE LICENSE" (Revision 42): 4 | * As long as you retain this notice you can do whatever you want with this 5 | * stuff. If we meet some day, and you think this stuff is worth it, you can 6 | * buy me a beer in return. 7 | * - Joe Romano and Will McMahan 8 | * ---------------------------------------------------------------------------- 9 | */ 10 | //@author Joe Romano 11 | //@author Will McMahan 12 | //@email joeromano@gmail.com 13 | //@brief digitalFilter.h - class to create IIR and FIR digital filter 14 | // coefficients in the Matlab (2010) style. This style being vectors 15 | // of coefficients for the numberator (b) and denominator (a) 16 | // respectively. 17 | // Please refer to the matlab documentation page for implementation 18 | // details: http://www.mathworks.com/access/helpdesk/help/techdoc/ref/filter.html 19 | 20 | #ifndef _DIGITALFILTER_H_ 21 | #define _DIGITALFILTER_H_ 22 | 23 | class digitalFilter 24 | { 25 | 26 | public: 27 | // Constructors 28 | digitalFilter(int filterOrder_userdef, bool isIIR); 29 | digitalFilter(int filterOrder_userdef, bool isIIR, float *b_userdef, float *a_userdef); 30 | 31 | ~digitalFilter(void); // Destructor 32 | 33 | float getNextFilteredValue(float u_current); 34 | 35 | protected: 36 | float *a, *b; // filter coefficients 37 | float *u, *x; // filter input and output states 38 | 39 | private: 40 | int filterOrder; 41 | bool IIR; 42 | 43 | }; 44 | 45 | #endif 46 | 47 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/launch/pr2_gripper_sensor_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/launch/pr2_gripper_sensor_controller.yaml: -------------------------------------------------------------------------------- 1 | l_gripper_sensor_controller: 2 | type: PR2GripperSensorController 3 | joint_name: l_gripper_joint 4 | accelerometer_name: l_gripper_motor 5 | left_pressure_sensor_name: l_gripper_motorl_finger_tip 6 | right_pressure_sensor_name: l_gripper_motorr_finger_tip 7 | close_speed: 0.02 8 | max_joint_effort: -1.0 9 | fingertip_force_limit: -1.0 10 | deformation_limit: 0.03 11 | force_lightest: 1.2 12 | hp_force_trigger: 0.1 13 | force_servo_force_tolerance: 0.3 14 | force_servo_velocity_tolerance: 0.001 15 | slip_servo_start_force: -1.0 16 | position_servo_position_tolerance: 0.01 17 | position_open: 0.09 18 | r_gripper_sensor_controller: 19 | type: PR2GripperSensorController 20 | joint_name: r_gripper_joint 21 | accelerometer_name: r_gripper_motor 22 | left_pressure_sensor_name: r_gripper_motorl_finger_tip 23 | right_pressure_sensor_name: r_gripper_motorr_finger_tip 24 | close_speed: 0.02 25 | max_joint_effort: -1.0 26 | fingertip_force_limit: -1.0 27 | deformation_limit: 0.03 28 | force_lightest: 1.2 29 | hp_force_trigger: 0.1 30 | force_servo_force_tolerance: 0.3 31 | force_servo_velocity_tolerance: 0.001 32 | slip_servo_start_force: -1.0 33 | position_servo_position_tolerance: 0.01 34 | position_open: 0.09 -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/launch/pr2_gripper_sensor_controller_left.yaml: -------------------------------------------------------------------------------- 1 | l_gripper_sensor_controller: 2 | type: PR2GripperSensorController 3 | joint_name: l_gripper_joint 4 | accelerometer_name: l_gripper_motor 5 | left_pressure_sensor_name: l_gripper_motorl_finger_tip 6 | right_pressure_sensor_name: l_gripper_motorr_finger_tip 7 | close_speed: 0.02 8 | max_joint_effort: -1.0 9 | fingertip_force_limit: -1.0 10 | deformation_limit: 0.03 11 | force_lightest: 1.2 12 | hp_force_trigger: 0.1 13 | force_servo_force_tolerance: 0.3 14 | force_servo_velocity_tolerance: 0.001 15 | slip_servo_start_force: -1.0 16 | position_servo_position_tolerance: 0.01 17 | position_open: 0.09 18 | r_gripper_sensor_controller: 19 | type: PR2GripperSensorController 20 | joint_name: r_gripper_joint 21 | accelerometer_name: r_gripper_motor 22 | left_pressure_sensor_name: r_gripper_motorl_finger_tip 23 | right_pressure_sensor_name: r_gripper_motorr_finger_tip 24 | close_speed: 0.02 25 | max_joint_effort: -1.0 26 | fingertip_force_limit: -1.0 27 | deformation_limit: 0.03 28 | force_lightest: 1.2 29 | hp_force_trigger: 0.1 30 | force_servo_force_tolerance: 0.3 31 | force_servo_velocity_tolerance: 0.001 32 | slip_servo_start_force: -1.0 33 | position_servo_position_tolerance: 0.01 34 | position_open: 0.09 -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/launch/pr2_gripper_sensor_controller_right.yaml: -------------------------------------------------------------------------------- 1 | l_gripper_sensor_controller: 2 | type: PR2GripperSensorController 3 | joint_name: l_gripper_joint 4 | accelerometer_name: l_gripper_motor 5 | left_pressure_sensor_name: l_gripper_motorl_finger_tip 6 | right_pressure_sensor_name: l_gripper_motorr_finger_tip 7 | close_speed: 0.02 8 | max_joint_effort: -1.0 9 | fingertip_force_limit: -1.0 10 | deformation_limit: 0.03 11 | force_lightest: 1.2 12 | hp_force_trigger: 0.1 13 | force_servo_force_tolerance: 0.3 14 | force_servo_velocity_tolerance: 0.001 15 | slip_servo_start_force: -1.0 16 | position_servo_position_tolerance: 0.01 17 | position_open: 0.09 18 | r_gripper_sensor_controller: 19 | type: PR2GripperSensorController 20 | joint_name: r_gripper_joint 21 | accelerometer_name: r_gripper_motor 22 | left_pressure_sensor_name: r_gripper_motorl_finger_tip 23 | right_pressure_sensor_name: r_gripper_motorr_finger_tip 24 | close_speed: 0.02 25 | max_joint_effort: -1.0 26 | fingertip_force_limit: -1.0 27 | deformation_limit: 0.03 28 | force_lightest: 1.2 29 | hp_force_trigger: 0.1 30 | force_servo_force_tolerance: 0.3 31 | force_servo_velocity_tolerance: 0.001 32 | slip_servo_start_force: -1.0 33 | position_servo_position_tolerance: 0.01 34 | position_open: 0.09 -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_sensor_controller is a real-time controller that uses the tactile pressure sensors on the PR2 gripper and accelerometer in the PR2 hand to do intelligent object grasping, holding, and placing 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_controller/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The pr2_gripper_sensor_controller package is a real-time controller that integrates signals from the PR2 hand-mounted accelerometer and finger-mounted pressure sensors with motor control of the gripper joint to do highly responsive sensing and low-latency closed-loop control. 5 | 6 | 7 | Joe Romano 8 | BSD 9 | 10 | http://ros.org/wiki/pr2_gripper_sensor_controller 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/.gitignore: -------------------------------------------------------------------------------- 1 | src/pr2_gripper_sensor_msgs/msg/ 2 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #for building our actions. This MUST come before rosbuild_init 13 | rosbuild_find_ros_package(actionlib_msgs) 14 | include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 15 | genaction() 16 | 17 | rosbuild_init() 18 | 19 | #set the default path for built executables to the "bin" directory 20 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 21 | #set the default path for built libraries to the "lib" directory 22 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 23 | 24 | #uncomment if you have defined messages 25 | rosbuild_genmsg() 26 | #uncomment if you have defined services 27 | #rosbuild_gensrv() 28 | 29 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/action/PR2GripperEventDetector.action: -------------------------------------------------------------------------------- 1 | # Event Detector action used to tell detect events happening on the 2 | # palm mounted accelerometer and finger pressure sensors 3 | 4 | #goal 5 | PR2GripperEventDetectorCommand command 6 | --- 7 | #results 8 | PR2GripperEventDetectorData data 9 | --- 10 | # feedback 11 | PR2GripperEventDetectorData data 12 | 13 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/action/PR2GripperFindContact.action: -------------------------------------------------------------------------------- 1 | # Contact action used to close fingers and find object contacts 2 | # quickly while still stopping fast in real-time to not damage 3 | # objects 4 | 5 | #goal 6 | PR2GripperFindContactCommand command 7 | --- 8 | #results 9 | PR2GripperFindContactData data 10 | --- 11 | # feedback 12 | PR2GripperFindContactData data 13 | 14 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/action/PR2GripperForceServo.action: -------------------------------------------------------------------------------- 1 | # Action to launch the gripper into force servoing mode 2 | 3 | #goals 4 | PR2GripperForceServoCommand command 5 | --- 6 | 7 | #result 8 | PR2GripperForceServoData data 9 | 10 | --- 11 | 12 | #feedback 13 | PR2GripperForceServoData data -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/action/PR2GripperGrab.action: -------------------------------------------------------------------------------- 1 | #goal 2 | PR2GripperGrabCommand command 3 | --- 4 | #result 5 | PR2GripperGrabData data 6 | --- 7 | 8 | #feedback 9 | PR2GripperGrabData data 10 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/action/PR2GripperRelease.action: -------------------------------------------------------------------------------- 1 | #goal 2 | PR2GripperReleaseCommand command 3 | --- 4 | #result 5 | PR2GripperReleaseData data 6 | --- 7 | 8 | #feedback 9 | PR2GripperReleaseData data 10 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/action/PR2GripperSlipServo.action: -------------------------------------------------------------------------------- 1 | # Action to launch the gripper into slip servoing mode 2 | 3 | #goals 4 | PR2GripperSlipServoCommand command 5 | --- 6 | 7 | #result 8 | PR2GripperSlipServoData data 9 | 10 | --- 11 | 12 | #feedback 13 | PR2GripperSlipServoData data -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_sensor_msgs is ... 6 | 7 | 14 | 15 | 16 | \section codeapi Code API 17 | 18 | 28 | 29 | 30 | */ -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | The pr2_gripper_sensor_msgs package contains various actions and messages that are used in the pr2_gripper_sensor* packages. The structure of the API used by pr2_gripper_sensor_action, and pr2_gripper_sensor_controller packages is as follows: 5 | 6 | Users will send a goal to an Action in the message format of PR2Gripper*Command (where * replaces the name of the particular Action from pr2_gripper_sensor_action). Feedback and Result information for the action is then returned in the format of PR2Gripper*Data. 7 | 8 | 9 | Joe Romano 10 | BSD 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/.gitignore: -------------------------------------------------------------------------------- 1 | PR2GripperEventDetectorActionFeedback.msg 2 | PR2GripperEventDetectorActionGoal.msg 3 | PR2GripperEventDetectorAction.msg 4 | PR2GripperEventDetectorActionResult.msg 5 | PR2GripperEventDetectorFeedback.msg 6 | PR2GripperEventDetectorGoal.msg 7 | PR2GripperEventDetectorResult.msg 8 | PR2GripperFindContactActionFeedback.msg 9 | PR2GripperFindContactActionGoal.msg 10 | PR2GripperFindContactAction.msg 11 | PR2GripperFindContactActionResult.msg 12 | PR2GripperFindContactFeedback.msg 13 | PR2GripperFindContactGoal.msg 14 | PR2GripperFindContactResult.msg 15 | PR2GripperForceServoActionFeedback.msg 16 | PR2GripperForceServoActionGoal.msg 17 | PR2GripperForceServoAction.msg 18 | PR2GripperForceServoActionResult.msg 19 | PR2GripperForceServoFeedback.msg 20 | PR2GripperForceServoGoal.msg 21 | PR2GripperForceServoResult.msg 22 | PR2GripperGrabActionFeedback.msg 23 | PR2GripperGrabActionGoal.msg 24 | PR2GripperGrabAction.msg 25 | PR2GripperGrabActionResult.msg 26 | PR2GripperGrabFeedback.msg 27 | PR2GripperGrabGoal.msg 28 | PR2GripperGrabResult.msg 29 | PR2GripperReleaseActionFeedback.msg 30 | PR2GripperReleaseActionGoal.msg 31 | PR2GripperReleaseAction.msg 32 | PR2GripperReleaseActionResult.msg 33 | PR2GripperReleaseFeedback.msg 34 | PR2GripperReleaseGoal.msg 35 | PR2GripperReleaseResult.msg 36 | PR2GripperSlipServoActionFeedback.msg 37 | PR2GripperSlipServoActionGoal.msg 38 | PR2GripperSlipServoAction.msg 39 | PR2GripperSlipServoActionResult.msg 40 | PR2GripperSlipServoGoal.msg 41 | PR2GripperSlipServoResult.msg 42 | PR2GripperSlipServoFeedback.msg -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperEventDetectorData.msg: -------------------------------------------------------------------------------- 1 | # Time the data was recorded at 2 | time stamp 3 | 4 | # true if the trigger conditions have been met 5 | # (see PR2GripperEventDetectorCommand) 6 | bool trigger_conditions_met 7 | 8 | # true if the pressure sensors detected a slip event 9 | # slip events occur when the finger pressure sensors 10 | # high-freq. content exceeds the slip_trigger_magnitude variable 11 | # (see PR2GripperEventDetectorCommand) 12 | bool slip_event 13 | 14 | # true if the hand-mounted accelerometer detected a contact acceleration 15 | # acceleration events occur when the palm accelerometer 16 | # high-freq. content exceeds the acc_trigger_magnitude variable 17 | # (see PR2GripperEventDetectorCommand) 18 | bool acceleration_event 19 | 20 | # the high-freq acceleration vector that was last seen (x,y,z) 21 | float64[3] acceleration_vector -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperFindContactCommand.msg: -------------------------------------------------------------------------------- 1 | # set true if you want to calibrate the fingertip sensors on the start 2 | # of the find_contact action. While this is not necessary (and 3 | # the default value will not calibrate the sensors) for best 4 | # performance it is recommended that you set this to true each time 5 | # you are calling find_contact and are confident the fingertips are 6 | # not touching anything 7 | # NOTE: SHOULD ONLY BE TRUE WHEN BOTH FINGERS ARE TOUCHING NOTHING 8 | bool zero_fingertip_sensors 9 | 10 | # the finger contact conditions that determine what our goal is 11 | # Leaving this field blank will result in the robot closing until 12 | # contact on BOTH fingers is achieved 13 | int8 contact_conditions 14 | 15 | # predefined values for the above contact_conditions variable 16 | int8 BOTH = 0 # both fingers must make contact 17 | int8 LEFT = 1 # just the left finger 18 | int8 RIGHT = 2 # just the right finger 19 | int8 EITHER = 3 # either finger, we don't care which 20 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperFindContactData.msg: -------------------------------------------------------------------------------- 1 | # Time the data was recorded at 2 | time stamp 3 | 4 | # true when our contact conditions have been met 5 | # (see PR2GripperFindContact command) 6 | bool contact_conditions_met 7 | 8 | # the finger contact conditions 9 | # true if the finger experienced a contact event 10 | # 11 | # contact events are defined as contact with the fingerpads 12 | # as either steady-state or high-freq force events 13 | bool left_fingertip_pad_contact 14 | bool right_fingertip_pad_contact 15 | 16 | # the force experinced by the finger Pads (N) 17 | # NOTE:this ignores data from the edges of the finger pressure 18 | float64 left_fingertip_pad_force 19 | float64 right_fingertip_pad_force 20 | 21 | # the current joint position (m) 22 | float64 joint_position 23 | 24 | # the virtual (parallel) joint effort (N) 25 | float64 joint_effort 26 | 27 | # the control state of our realtime controller 28 | PR2GripperSensorRTState rtstate -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperForceServoCommand.msg: -------------------------------------------------------------------------------- 1 | # the amount of fingertip force (in Newtons) to apply. 2 | # NOTE: the joint will squeeze until each finger reaches this level 3 | # values < 0 (opening force) are ignored 4 | # 5 | # 10 N can crack an egg or crush a soda can. 6 | # 15 N can firmly pick up a can of soup. 7 | # Experiment on your own. 8 | # 9 | float64 fingertip_force -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperForceServoData.msg: -------------------------------------------------------------------------------- 1 | # Time the data was recorded at 2 | time stamp 3 | 4 | # the force experienced by the finger Pads (N) 5 | # NOTE:this ignores data from the edges of the finger pressure 6 | float64 left_fingertip_pad_force 7 | float64 right_fingertip_pad_force 8 | 9 | # the current gripper virtual parallel joint effort (in N) 10 | float64 joint_effort 11 | 12 | # true when the gripper is no longer moving 13 | # and we have reached the desired force level 14 | bool force_achieved 15 | 16 | 17 | # the control state of our realtime controller 18 | PR2GripperSensorRTState rtstate 19 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperGrabCommand.msg: -------------------------------------------------------------------------------- 1 | # The gain to use to evaluate how hard an object should be 2 | # grasped after it is contacted. This is based on hardness 3 | # estimation as outlined in TRO paper (see wiki). 4 | # 5 | # Try 0.03 6 | # 7 | # Units (N/(m/s^2)) 8 | float64 hardness_gain 9 | -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperGrabData.msg: -------------------------------------------------------------------------------- 1 | # the control state of our realtime controller 2 | PR2GripperSensorRTState rtstate -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperPressureData.msg: -------------------------------------------------------------------------------- 1 | # Note: This message is intended for internal package use only and is NOT a part of the public API. This data is not publicaly published in ROS. 2 | 3 | # the pressure array for the left and right fingers 4 | float64[22] pressure_left 5 | float64[22] pressure_right 6 | 7 | float64 rostime -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperReleaseCommand.msg: -------------------------------------------------------------------------------- 1 | # the event conditions we would like to trigger the robot to release on 2 | PR2GripperEventDetectorCommand event -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperReleaseData.msg: -------------------------------------------------------------------------------- 1 | # the control state of our realtime controller 2 | PR2GripperSensorRTState rtstate -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperSensorRTState.msg: -------------------------------------------------------------------------------- 1 | # the control state of our realtime controller 2 | int8 realtime_controller_state 3 | 4 | # predefined values to indicate our realtime_controller_state 5 | int8 DISABLED = 0 6 | int8 POSITION_SERVO = 3 7 | int8 FORCE_SERVO = 4 8 | int8 FIND_CONTACT = 5 9 | int8 SLIP_SERVO = 6 -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperSensorRawData.msg: -------------------------------------------------------------------------------- 1 | # NOTE: This message is only for debugging purposes. It is not intended for API usage - and is not published under release code. 2 | 3 | # Standard ROS header 4 | time stamp 5 | 6 | # corrected for zero contact 7 | float64 left_finger_pad_force 8 | float64 right_finger_pad_force 9 | 10 | # filtered values : high pass filter at 5 Hz after correcting for zero contact 11 | float64 left_finger_pad_force_filtered 12 | float64 right_finger_pad_force_filtered 13 | 14 | # corrected for zero contact 15 | float64[22] left_finger_pad_forces 16 | float64[22] right_finger_pad_forces 17 | 18 | # filtered values : high pass filter at 5 Hz after correcting for zero contact 19 | float64[22] left_finger_pad_forces_filtered 20 | float64[22] right_finger_pad_forces_filtered 21 | 22 | # raw acceleration values 23 | float64 acc_x_raw 24 | float64 acc_y_raw 25 | float64 acc_z_raw 26 | 27 | # filtered acceleration values 28 | float64 acc_x_filtered 29 | float64 acc_y_filtered 30 | float64 acc_z_filtered 31 | 32 | # boolean variables indicating whether contact exists or not 33 | bool left_contact 34 | bool right_contact -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperSlipServoCommand.msg: -------------------------------------------------------------------------------- 1 | # this command is currently blank, but may see additional variable 2 | # additions in the future 3 | 4 | # see the param server documentation for a list of variables that effect 5 | # slip servo performance -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/msg/PR2GripperSlipServoData.msg: -------------------------------------------------------------------------------- 1 | # time the data was recorded at 2 | time stamp 3 | 4 | # the amount of deformation from action start (in meters) 5 | float64 deformation 6 | 7 | # the force experinced by the finger Pads (N) 8 | # NOTE:this ignores data from the edges of the finger pressure 9 | float64 left_fingertip_pad_force 10 | float64 right_fingertip_pad_force 11 | 12 | # the current virtual parallel joint effort of the gripper (in N) 13 | float64 joint_effort 14 | 15 | # true if the object recently slipped 16 | bool slip_detected 17 | 18 | # true if we are at or exceeding the deformation limit 19 | # (see wiki page and param server for more info) 20 | bool deformation_limit_reached 21 | 22 | # true if we are at or exceeding our force 23 | # (see wiki page and param server for more info) 24 | bool fingertip_force_limit_reached 25 | 26 | # true if the controller thinks the gripper is empty 27 | # (see wiki page for more info) 28 | bool gripper_empty 29 | 30 | # the control state of our realtime controller 31 | PR2GripperSensorRTState rtstate -------------------------------------------------------------------------------- /manipulation/pr2_gripper_sensor_msgs/src/pr2_gripper_sensor_msgs/__init__.py: -------------------------------------------------------------------------------- 1 | #autogenerated by ROS python message generators -------------------------------------------------------------------------------- /manipulation/pr2_interactive_gripper_pose_action/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 5 | find_package(Eigen REQUIRED) 6 | include_directories(SYSTEM ${EIGEN_INCLUDE_DIRS}) 7 | 8 | # Set the build type. Options are: 9 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 10 | # Debug : w/ debug symbols, w/o optimization 11 | # Release : w/o debug symbols, w/ optimization 12 | # RelWithDebInfo : w/ debug symbols, w/ optimization 13 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 14 | #set(ROS_BUILD_TYPE RelWithDebInfo) 15 | 16 | 17 | #generate the actions 18 | #rosbuild_find_ros_package(actionlib_msgs) 19 | #include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 20 | #genaction() 21 | 22 | rosbuild_init() 23 | 24 | #set the default path for built executables to the "bin" directory 25 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 26 | #set the default path for built libraries to the "lib" directory 27 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 28 | 29 | #uncomment if you have defined messages 30 | #rosbuild_genmsg() 31 | #uncomment if you have defined services 32 | #rosbuild_gensrv() 33 | 34 | rosbuild_add_executable(pr2_interactive_gripper_pose_action 35 | src/pr2_interactive_gripper_pose_action.cpp 36 | manifest.xml 37 | ) 38 | 39 | rosbuild_add_executable(pr2_interactive_nav_action 40 | src/pr2_interactive_nav_action.cpp 41 | manifest.xml 42 | ) 43 | -------------------------------------------------------------------------------- /manipulation/pr2_interactive_gripper_pose_action/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_interactive_gripper_pose_action/launch/pr2_interactive_gripper_pose_action.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /manipulation/pr2_interactive_gripper_pose_action/launch/pr2_interactive_nav_action.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /manipulation/pr2_interactive_gripper_pose_action/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_marker_controls is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_interactive_gripper_pose_action/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This package provides an action node that, when called, 5 | displays an interactive marker in Rviz (or any other client) 6 | to interactively select a pose for the pr2 gripper. 7 | 8 | 9 | Adam Leeper 10 | BSD 11 | 12 | http://ros.org/wiki/pr2_marker_control 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/.gitignore: -------------------------------------------------------------------------------- 1 | src/pr2_manipulation_controllers/msg/ 2 | src/pr2_manipulation_controllers/srv/ -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 4 | 5 | # Set the build type. Options are: 6 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 7 | # Debug : w/ debug symbols, w/o optimization 8 | # Release : w/o debug symbols, w/ optimization 9 | # RelWithDebInfo : w/ debug symbols, w/ optimization 10 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 11 | #set(ROS_BUILD_TYPE RelWithDebInfo) 12 | 13 | rosbuild_init() 14 | 15 | #set the default path for built executables to the "bin" directory 16 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 17 | #set the default path for built libraries to the "lib" directory 18 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 19 | 20 | #uncomment if you have defined messages 21 | rosbuild_genmsg() 22 | #uncomment if you have defined services 23 | rosbuild_gensrv() 24 | 25 | find_package(Eigen REQUIRED) 26 | include_directories(SYSTEM ${Eigen_INCLUDE_DIRS}) 27 | add_definitions(${EIGEN_DEFINITIONS}) 28 | 29 | rosbuild_add_library(pr2_manipulation_controllers 30 | src/cartesian_trajectory_controller.cpp 31 | src/jinv_teleop_controller.cpp 32 | src/jt_task_controller.cpp 33 | src/jinv_experimental_controller.cpp 34 | ) 35 | 36 | 37 | #common commands for building c++ executables and libraries 38 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 39 | #target_link_libraries(${PROJECT_NAME} another_library) 40 | #rosbuild_add_boost_directories() 41 | #rosbuild_link_boost(${PROJECT_NAME} thread) 42 | #rosbuild_add_executable(example examples/example.cpp) 43 | #target_link_libraries(example ${PROJECT_NAME}) 44 | -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 7 | 8 | 10 | 11 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_manipulation_controllers is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/msg/JTTaskControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped x 3 | geometry_msgs/PoseStamped x_desi 4 | geometry_msgs/PoseStamped x_desi_filtered 5 | geometry_msgs/Twist x_err 6 | geometry_msgs/Twist xd 7 | geometry_msgs/Twist xd_desi 8 | geometry_msgs/Wrench F 9 | float64[] tau_pose 10 | float64[] tau_posture 11 | float64[] tau 12 | std_msgs/Float64MultiArray J 13 | std_msgs/Float64MultiArray N 14 | 15 | # Environmental stiffness 16 | float64 df 17 | float64 dx 18 | float64 Df 19 | float64 Dx 20 | float64 stiffness 21 | float64 compliance -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/msg/JTTeleopControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped x 3 | geometry_msgs/PoseStamped x_desi 4 | geometry_msgs/PoseStamped x_desi_filtered 5 | geometry_msgs/Twist x_err 6 | geometry_msgs/Twist xd 7 | geometry_msgs/Twist xd_desi 8 | geometry_msgs/Wrench F 9 | float64[] tau_pose 10 | float64[] tau_posture 11 | float64[] tau 12 | std_msgs/Float64MultiArray J 13 | std_msgs/Float64MultiArray N 14 | 15 | # Environmental stiffness 16 | float64 df 17 | float64 dx 18 | float64 Df 19 | float64 Dx 20 | float64 stiffness 21 | float64 compliance -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/msg/JinvExperimentalControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped x 3 | geometry_msgs/PoseStamped x_desi 4 | geometry_msgs/PoseStamped x_desi_filtered 5 | geometry_msgs/Twist x_err 6 | geometry_msgs/Twist xd 7 | geometry_msgs/Twist xd_desi 8 | geometry_msgs/Wrench F 9 | float64[] q_proxy 10 | float64[] qd_pose 11 | float64[] qd_posture 12 | float64[] qd_posture_raw 13 | float64[] qd_desi 14 | float64[] tau 15 | std_msgs/Float64MultiArray J 16 | std_msgs/Float64MultiArray N 17 | 18 | float64[] J_singular_values 19 | 20 | # Environmental stiffness 21 | float64 df 22 | float64 dx 23 | float64 Df 24 | float64 Dx 25 | float64 stiffness 26 | float64 compliance 27 | -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/msg/JinvTeleopControllerState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/PoseStamped x 3 | geometry_msgs/PoseStamped x_desi 4 | geometry_msgs/PoseStamped x_desi_filtered 5 | geometry_msgs/Twist x_err 6 | geometry_msgs/Twist xd 7 | geometry_msgs/Twist xd_desi 8 | geometry_msgs/Wrench F 9 | float64[] q_proxy 10 | float64[] qd_pose 11 | float64[] qd_posture 12 | float64[] qd_posture_raw 13 | float64[] qd_desi 14 | float64[] tau 15 | std_msgs/Float64MultiArray J 16 | std_msgs/Float64MultiArray N 17 | 18 | # Environmental stiffness 19 | float64 df 20 | float64 dx 21 | float64 Df 22 | float64 Dx 23 | float64 stiffness 24 | float64 compliance 25 | -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/src/pr2_manipulation_controllers/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/manipulation/pr2_manipulation_controllers/src/pr2_manipulation_controllers/__init__.py -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/srv/CheckMoving.srv: -------------------------------------------------------------------------------- 1 | --- 2 | uint8 ismoving #1 if moving, 0 if not moving 3 | -------------------------------------------------------------------------------- /manipulation/pr2_manipulation_controllers/srv/MoveToPose.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped pose 2 | geometry_msgs/Twist tolerance 3 | --- 4 | -------------------------------------------------------------------------------- /manipulation/pr2_marker_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #generate the actions 13 | #rosbuild_find_ros_package(actionlib_msgs) 14 | #include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 15 | #genaction() 16 | 17 | rosbuild_init() 18 | 19 | #set the default path for built executables to the "bin" directory 20 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 21 | #set the default path for built libraries to the "lib" directory 22 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 23 | 24 | #uncomment if you have defined messages 25 | #rosbuild_genmsg() 26 | #uncomment if you have defined services 27 | #rosbuild_gensrv() 28 | 29 | #rosbuild_add_executable(pr2_marker_control 30 | #src/cloud_handler.cpp 31 | #src/marker_control.cpp 32 | #src/marker_control_node.cpp 33 | #manifest.xml 34 | #) 35 | 36 | rosbuild_add_library(pr2_marker_control_lib 37 | src/cloud_handler.cpp 38 | src/marker_control.cpp 39 | src/generate_robot_model.cpp 40 | ) 41 | 42 | rosbuild_add_executable(pr2_marker_control 43 | src/marker_control_node.cpp 44 | manifest.xml 45 | ) 46 | target_link_libraries(pr2_marker_control pr2_marker_control_lib) -------------------------------------------------------------------------------- /manipulation/pr2_marker_control/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_marker_control/TODO: -------------------------------------------------------------------------------- 1 | - add seamless switching between cartesian control and joint controller. 2 | - add head control... (target marker, or possibly drag handle.) 3 | - Control to send goals to nav stack. 4 | - Ability to reset head target... 5 | - Individual joint control 6 | 7 | MAKE THE ARM MOVE: 8 | - draw mesh instead of points when object model is known 9 | - at least figure out why the head action times out 10 | - model_object should subscribe only when active (and the filter should be idle when not subscribed-to) 11 | -------------------------------------------------------------------------------- /manipulation/pr2_marker_control/launch/desktop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /manipulation/pr2_marker_control/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_marker_controls is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_marker_control/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A PR2 application that uses Interactive Markers to provide 5 | a point-and-click inteface for controlling the PR2 in Rviz. 6 | 7 | 8 | Adam Leeper 9 | BSD 10 | 11 | http://ros.org/wiki/pr2_marker_control 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | rosbuild_add_roslaunch_check(launch/c_jexp.launch ROBOT=sim) 20 | rosbuild_add_roslaunch_check(launch/c_jinv.launch ROBOT=sim) 21 | rosbuild_add_roslaunch_check(launch/c_jtask.launch ROBOT=sim) 22 | rosbuild_add_roslaunch_check(launch/c_jt.launch ROBOT=sim) 23 | rosbuild_add_roslaunch_check(launch/c_rmrc.launch ROBOT=sim) 24 | rosbuild_add_roslaunch_check(launch/laser+stereo-perception.launch ROBOT=sim) 25 | rosbuild_add_roslaunch_check(launch/pr2_manipulation.launch ROBOT=sim) 26 | rosbuild_add_roslaunch_check(launch/pr2_manipulation_prerequisites.launch ROBOT=sim) 27 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/config/collision_map_sources_laser.yaml: -------------------------------------------------------------------------------- 1 | cloud_sources: 2 | - name: /tilt_scan_cloud_filtered2 3 | frame_subsample: 1 4 | point_subsample: 1 5 | sensor_frame: laser_tilt_link 6 | 7 | camera_info_topic: /narrow_stereo_textured/left/camera_info 8 | camera_stereo_offset_left: 128 9 | camera_stereo_offset_right: 0 10 | 11 | 12 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/config/lcg_left_pr2_hand_descriptions.yaml: -------------------------------------------------------------------------------- 1 | hand_description: 2 | right_arm: 3 | hand_frame: r_wrist_roll_link 4 | robot_frame: base_link 5 | attached_objects_name: attached 6 | attach_link: r_gripper_r_finger_tip_link 7 | hand_group_name: r_end_effector 8 | arm_group_name: right_arm 9 | hand_database_name: LCG 10 | hand_joints: [r_gripper_l_finger_joint,r_gripper_r_finger_joint,r_gripper_r_finger_tip_joint,r_gripper_l_finger_tip_joint] 11 | hand_touch_links: [r_gripper_palm_link,r_gripper_r_finger_tip_link,r_gripper_l_finger_tip_link,r_gripper_l_finger_link,r_gripper_r_finger_link,r_wrist_roll_link] 12 | hand_fingertip_links: [r_gripper_r_finger_tip_link,r_gripper_l_finger_tip_link] 13 | hand_approach_direction: [1.0,0.0,0.0] 14 | arm_joints: [r_shoulder_pan_joint,r_shoulder_lift_joint,r_upper_arm_roll_joint,r_elbow_flex_joint,r_forearm_roll_joint,r_wrist_flex_joint,r_wrist_roll_joint] 15 | left_arm: 16 | hand_frame: l_wrist_roll_link 17 | robot_frame: base_link 18 | attached_objects_name: attached 19 | attach_link: l_gripper_r_finger_tip_link 20 | hand_group_name: l_end_effector 21 | arm_group_name: left_arm 22 | hand_database_name: LCG 23 | hand_joints: [l_gripper_l_finger_joint,l_gripper_l_finger_tip_joint,l_gripper_r_finger_joint,l_gripper_r_finger_tip_joint] 24 | hand_touch_links: [l_gripper_palm_link,l_gripper_r_finger_tip_link,l_gripper_l_finger_tip_link,l_gripper_l_finger_link,l_gripper_r_finger_link,l_wrist_roll_link] 25 | hand_fingertip_links: [l_gripper_r_finger_tip_link,l_gripper_l_finger_tip_link] 26 | hand_approach_direction: [1.0,0.0,0.0] 27 | arm_joints: [l_shoulder_pan_joint,l_shoulder_lift_joint,l_upper_arm_roll_joint,l_elbow_flex_joint,l_forearm_roll_joint,l_wrist_flex_joint,l_wrist_roll_joint] 28 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/config/pr2_arm_configurations.yaml: -------------------------------------------------------------------------------- 1 | 2 | arm_configurations: 3 | side: 4 | position: 5 | right_arm: [-2.135, -0.02, -1.64, -2.07, -1.64, -1.680, 1.398] 6 | left_arm: [ 2.135, -0.02, 1.64, -2.07, 1.64, -1.680, 1.398] 7 | trajectory: 8 | right_arm: [-0.968, 0.129, -0.554, -1.891, -1.786, -1.127, 0.501, -2.135, -0.02, -1.64, -2.07, -1.64, -1.680, 1.398] 9 | left_arm: [ 0.968, 0.129, 0.554, -1.891, 1.786, -1.127, 0.501, 2.135, -0.02, 1.64, -2.07, 1.64, -1.680, 1.398] 10 | 11 | front: 12 | position: 13 | right_arm: [0.0, 1.203, 0.0, -2.105, -3.13, -1.680, 1.398] 14 | left_arm: [0.0, 1.203, 0.0, -2.105, 3.13, -1.680, 1.398] 15 | trajectory: 16 | right_arm: [-0.968, 0.729, -0.554, -1.891, -1.786, -1.127, 0.501, 0.0, 1.203, 0.0, -2.105, -3.13, -1.680, 1.398] 17 | left_arm: [ 0.968, 0.729, 0.554, -1.891, 1.786, -1.127, 0.501, 0.0, 1.203, 0.0, -2.105, 3.13, -1.680, 1.398] 18 | 19 | above: 20 | position: 21 | right_arm: [-0.622617, -0.040037, -1.505380, -1.628758, -1.608914, -1.627983, 0.564746] 22 | left_arm: [ 0.622617, 0.040037, 1.505380, -1.628758, 1.608914, -1.627983, 0.564746] 23 | trajectory: 24 | right_arm: [-0.968, 0.129, -0.554, -1.891, -1.786, -1.127, 0.501, -0.622617, -0.040037, -1.505380, -1.628758, -1.608914, -1.627983, 0.564746] 25 | left_arm: [ 0.968, 0.129, 0.554, -1.891, 1.786, -1.127, 0.501, 0.622617, 0.040037, 1.505380, -1.628758, 1.608914, -1.627983, 0.564746] 26 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/config/pr2_hand_descriptions.yaml: -------------------------------------------------------------------------------- 1 | hand_description: 2 | right_arm: 3 | hand_frame: r_wrist_roll_link 4 | robot_frame: base_link 5 | attached_objects_name: attached 6 | attach_link: r_gripper_r_finger_tip_link 7 | hand_group_name: r_end_effector 8 | arm_group_name: right_arm 9 | hand_database_name: WILLOW_GRIPPER_2010 10 | hand_joints: [r_gripper_l_finger_joint,r_gripper_r_finger_joint,r_gripper_r_finger_tip_joint,r_gripper_l_finger_tip_joint] 11 | hand_touch_links: [r_gripper_palm_link,r_gripper_r_finger_tip_link,r_gripper_l_finger_tip_link,r_gripper_l_finger_link,r_gripper_r_finger_link,r_wrist_roll_link] 12 | hand_fingertip_links: [r_gripper_r_finger_tip_link,r_gripper_l_finger_tip_link] 13 | hand_approach_direction: [1.0,0.0,0.0] 14 | arm_joints: [r_shoulder_pan_joint,r_shoulder_lift_joint,r_upper_arm_roll_joint,r_elbow_flex_joint,r_forearm_roll_joint,r_wrist_flex_joint,r_wrist_roll_joint] 15 | left_arm: 16 | hand_frame: l_wrist_roll_link 17 | robot_frame: base_link 18 | attached_objects_name: attached 19 | attach_link: l_gripper_r_finger_tip_link 20 | hand_group_name: l_end_effector 21 | arm_group_name: left_arm 22 | hand_database_name: WILLOW_GRIPPER_2010 23 | hand_joints: [l_gripper_l_finger_joint,l_gripper_r_finger_joint,l_gripper_r_finger_tip_joint,l_gripper_l_finger_tip_joint] 24 | hand_touch_links: [l_gripper_palm_link,l_gripper_r_finger_tip_link,l_gripper_l_finger_tip_link,l_gripper_l_finger_link,l_gripper_r_finger_link,l_wrist_roll_link] 25 | hand_fingertip_links: [l_gripper_r_finger_tip_link,l_gripper_l_finger_tip_link] 26 | hand_approach_direction: [1.0,0.0,0.0] 27 | arm_joints: [l_shoulder_pan_joint,l_shoulder_lift_joint,l_upper_arm_roll_joint,l_elbow_flex_joint,l_forearm_roll_joint,l_wrist_flex_joint,l_wrist_roll_joint] 28 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_object_manipulation_launch is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_launch/manifest.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Contains launch and configuration files for starting the 5 | manipulation pipeline, along with all needed controllers, 6 | planners, etc., on the PR2 robot. 7 | 8 | 9 | Matei Ciocarlie 10 | BSD 11 | 12 | http://ros.org/wiki/pr2_object_manipulation_launch 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/.gitignore: -------------------------------------------------------------------------------- 1 | src/pr2_object_manipulation_msgs/msg/ 2 | src/pr2_object_manipulation_msgs/srv/ -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #generate the actions 13 | rosbuild_find_ros_package(actionlib_msgs) 14 | include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 15 | genaction() 16 | 17 | rosbuild_init() 18 | 19 | #set the default path for built executables to the "bin" directory 20 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 21 | #set the default path for built libraries to the "lib" directory 22 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 23 | 24 | #uncomment if you have defined messages 25 | rosbuild_genmsg() 26 | #uncomment if you have defined services 27 | rosbuild_gensrv() 28 | 29 | #common commands for building c++ executables and libraries 30 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 31 | #target_link_libraries(${PROJECT_NAME} another_library) 32 | #rosbuild_add_boost_directories() 33 | #rosbuild_link_boost(${PROJECT_NAME} thread) 34 | #rosbuild_add_executable(example examples/example.cpp) 35 | #target_link_libraries(example ${PROJECT_NAME}) 36 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/GetGripperPose.action: -------------------------------------------------------------------------------- 1 | 2 | # for which arm are we requesting a pose 3 | # useful for knowing which arm to initialize from when seed is empty 4 | string arm_name 5 | 6 | # a seed position for the ghosted gripper. If emtpy (quaternion of norm 0) 7 | # the ghosted gripper will be initialized at the current position of the gripper 8 | geometry_msgs/PoseStamped gripper_pose 9 | float32 gripper_opening 10 | 11 | # An object that the gripper is holding. May be empty. 12 | manipulation_msgs/GraspableObject object 13 | 14 | # how we are holding the object, if any. 15 | manipulation_msgs/Grasp grasp 16 | 17 | --- 18 | # the returned gripper pose, assuming the user has clicked "accept" 19 | # if the user clicks "cancel" the action aborts 20 | geometry_msgs/PoseStamped gripper_pose 21 | float32 gripper_opening 22 | --- 23 | 24 | # the feedback gripper pose, which is basically raw and unfiltered but may be useful 25 | geometry_msgs/PoseStamped gripper_pose 26 | float32 gripper_opening 27 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/GetNavPose.action: -------------------------------------------------------------------------------- 1 | 2 | # a seed position for the pose marker. If empty (quaternion of norm 0) 3 | # the marker will be initialized at the current pose of the header frame. 4 | geometry_msgs/PoseStamped starting_pose 5 | 6 | # The maximum in-plane distance that the action will allow 7 | float32 max_distance 8 | --- 9 | # the returned pose, assuming the user has clicked "accept" 10 | # if the user clicks "cancel" the action aborts 11 | geometry_msgs/PoseStamped pose 12 | 13 | --- 14 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/GetPose.action: -------------------------------------------------------------------------------- 1 | 2 | # a seed position for the pose marker. If empty (quaternion of norm 0) 3 | # the marker will be initialized at the current pose of the header frame. 4 | geometry_msgs/PoseStamped starting_pose 5 | 6 | --- 7 | # the returned pose, assuming the user has clicked "accept" 8 | # if the user clicks "cancel" the action aborts 9 | geometry_msgs/PoseStamped pose 10 | 11 | --- 12 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/IMGUI.action: -------------------------------------------------------------------------------- 1 | 2 | IMGUIOptions options 3 | IMGUICommand command 4 | 5 | --- 6 | 7 | #empty result 8 | object_manipulation_msgs/ManipulationResult result 9 | 10 | --- 11 | 12 | string status 13 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/PickupIMObject.action: -------------------------------------------------------------------------------- 1 | # objects are numbered in the same order as the GraspableObjectsList (0 start) 2 | int32 object_id 3 | 4 | # 0=right, 1=left arm 5 | int32 arm_selection 6 | 7 | --- 8 | 9 | --- 10 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/PoseStampedScripted.action: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped pose_stamped 2 | 3 | --- 4 | 5 | string result 6 | 7 | --- 8 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/RunScript.action: -------------------------------------------------------------------------------- 1 | string action_name 2 | string group_name 3 | --- 4 | string result 5 | --- 6 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/action/TestGripperPose.action: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseStamped[] gripper_poses 2 | float32[] gripper_openings 3 | --- 4 | bool[] valid 5 | --- -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_im_msgs is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pr2_im_msgs 5 | 6 | 7 | Matei Ciocarlie 8 | BSD 9 | 10 | http://ros.org/wiki/pr2_im_msgs 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/msg/.gitignore: -------------------------------------------------------------------------------- 1 | GetGripperPoseActionFeedback.msg 2 | GetGripperPoseActionGoal.msg 3 | GetGripperPoseAction.msg 4 | GetGripperPoseActionResult.msg 5 | GetGripperPoseFeedback.msg 6 | GetGripperPoseGoal.msg 7 | GetGripperPoseResult.msg 8 | GetNavPoseActionFeedback.msg 9 | GetNavPoseActionGoal.msg 10 | GetNavPoseAction.msg 11 | GetNavPoseActionResult.msg 12 | GetNavPoseFeedback.msg 13 | GetNavPoseGoal.msg 14 | GetNavPoseResult.msg 15 | GetPoseActionFeedback.msg 16 | GetPoseActionGoal.msg 17 | GetPoseAction.msg 18 | GetPoseActionResult.msg 19 | GetPoseFeedback.msg 20 | GetPoseGoal.msg 21 | GetPoseResult.msg 22 | IMGUIActionFeedback.msg 23 | IMGUIActionGoal.msg 24 | IMGUIAction.msg 25 | IMGUIActionResult.msg 26 | IMGUIFeedback.msg 27 | IMGUIGoal.msg 28 | IMGUIOptions.msg 29 | IMGUIResult.msg 30 | PoseStampedScriptedActionFeedback.msg 31 | PoseStampedScriptedActionGoal.msg 32 | PoseStampedScriptedAction.msg 33 | PoseStampedScriptedActionResult.msg 34 | PoseStampedScriptedFeedback.msg 35 | PoseStampedScriptedGoal.msg 36 | PoseStampedScriptedResult.msg 37 | RunScriptActionFeedback.msg 38 | RunScriptActionGoal.msg 39 | RunScriptAction.msg 40 | RunScriptActionResult.msg 41 | RunScriptFeedback.msg 42 | RunScriptGoal.msg 43 | RunScriptResult.msg 44 | TestGripperPoseActionFeedback.msg 45 | TestGripperPoseActionGoal.msg 46 | TestGripperPoseAction.msg 47 | TestGripperPoseActionResult.msg 48 | TestGripperPoseFeedback.msg 49 | TestGripperPoseGoal.msg 50 | TestGripperPoseResult.msg 51 | PickupIMObjectAction.msg 52 | PickupIMObjectActionFeedback.msg 53 | PickupIMObjectActionGoal.msg 54 | PickupIMObjectActionResult.msg 55 | PickupIMObjectFeedback.msg 56 | PickupIMObjectGoal.msg 57 | PickupIMObjectResult.msg 58 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/msg/CameraFocus.msg: -------------------------------------------------------------------------------- 1 | # Requests that the RViz 3D camera be focused at this point 2 | # still under construction 3 | 4 | geometry_msgs/PointStamped focal_point -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/msg/IMGUIAdvancedOptions.msg: -------------------------------------------------------------------------------- 1 | 2 | bool reactive_grasping 3 | bool reactive_force 4 | bool reactive_place 5 | int32 lift_steps 6 | int32 retreat_steps 7 | int32 lift_direction_choice 8 | int32 desired_approach 9 | int32 min_approach 10 | float32 max_contact_force 11 | bool find_alternatives 12 | bool always_plan_grasps 13 | bool cycle_gripper_opening 14 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/msg/IMGUICommand.msg: -------------------------------------------------------------------------------- 1 | 2 | int32 PICKUP = 0 3 | int32 PLACE = 1 4 | int32 PLANNED_MOVE = 2 5 | int32 RESET = 3 6 | int32 MOVE_ARM = 4 7 | int32 LOOK_AT_TABLE = 5 8 | int32 MODEL_OBJECT = 6 9 | int32 MOVE_GRIPPER = 7 10 | int32 SCRIPTED_ACTION = 8 11 | int32 STOP_NAV = 9 12 | 13 | int32 command 14 | string script_name 15 | string script_group_name 16 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/msg/IMGUIOptions.msg: -------------------------------------------------------------------------------- 1 | 2 | # collision checking enabled 3 | bool collision_checked 4 | 5 | # 0=call gripper click 6 | # 1=grasp the provided graspable object 7 | int32 grasp_selection 8 | 9 | # 0=right, 1=left arm 10 | int32 arm_selection 11 | 12 | # for RESET commands 13 | # 0=reset collision objects 14 | # 1=reset attached objects 15 | int32 reset_choice 16 | 17 | # for MOVE_ARM commands 18 | # 0=side 19 | # 1=front 20 | # 2=side handoff 21 | int32 arm_action_choice 22 | 23 | # for MOVE_ARM commands 24 | # 0=open-loop 25 | # 1=with planner 26 | int32 arm_planner_choice 27 | 28 | # for MOVE_GRIPPER commands 29 | # opening of gripper (0=closed..100=open) 30 | int32 gripper_slider_position 31 | 32 | # used if grasp_selection == 1 33 | manipulation_msgs/GraspableObject selected_object 34 | 35 | # indicates obstacles that can be moved during grasping 36 | # presumably, the operator has marked these in some fashion 37 | manipulation_msgs/GraspableObject[] movable_obstacles 38 | 39 | # more options.. 40 | IMGUIAdvancedOptions adv_options 41 | -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/msg/ImageClick.msg: -------------------------------------------------------------------------------- 1 | # Info about a user click on an image 2 | 3 | # The ray in 3d space resulting from the click 4 | Ray ray 5 | 6 | # The frame_if of the camera that the click was in 7 | string camera_frame_id -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/msg/Ray.msg: -------------------------------------------------------------------------------- 1 | # Represents a ray through space. 2 | # Could probably move to a more general location 3 | 4 | Header header 5 | 6 | # the origin of the ray 7 | geometry_msgs/Point origin 8 | 9 | # the direction of the ray 10 | geometry_msgs/Vector3 direction -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/src/pr2_object_manipulation_msgs/__init__.py: -------------------------------------------------------------------------------- 1 | #autogenerated by ROS python message generators -------------------------------------------------------------------------------- /manipulation/pr2_object_manipulation_msgs/srv/ActionInfo.srv: -------------------------------------------------------------------------------- 1 | string group_name 2 | --- 3 | string[] services 4 | -------------------------------------------------------------------------------- /manipulation/pr2_wrappers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #generate the actions 13 | 14 | rosbuild_init() 15 | 16 | #set the default path for built executables to the "bin" directory 17 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 18 | #set the default path for built libraries to the "lib" directory 19 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 20 | 21 | #uncomment if you have defined messages 22 | #rosbuild_genmsg() 23 | #uncomment if you have defined services 24 | #rosbuild_gensrv() 25 | 26 | #common commands for building c++ executables and libraries 27 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 28 | #target_link_libraries(${PROJECT_NAME} another_library) 29 | #rosbuild_add_boost_directories() 30 | #rosbuild_link_boost(${PROJECT_NAME} thread) 31 | #rosbuild_add_executable(example examples/example.cpp) 32 | #target_link_libraries(example ${PROJECT_NAME}) 33 | 34 | rosbuild_add_library(pr2_wrappers 35 | include/pr2_wrappers/gripper_controller.h 36 | src/gripper_controller.cpp 37 | include/pr2_wrappers/torso_client.h 38 | src/torso_client.cpp 39 | include/pr2_wrappers/base_client.h 40 | src/base_client.cpp 41 | include/pr2_wrappers/tuck_arms_client.h 42 | src/tuck_arms_client.cpp 43 | include/pr2_wrappers/plugs_client.h 44 | src/plugs_client.cpp 45 | manifest.xml 46 | ) 47 | 48 | 49 | # Check for SSE 50 | rosbuild_check_for_sse() 51 | 52 | 53 | -------------------------------------------------------------------------------- /manipulation/pr2_wrappers/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/pr2_wrappers/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A library of useful PR2 interface classes. 5 | 6 | 7 | Adam Leeper 8 | BSD 9 | 10 | http://ros.org/wiki/pr2_wrappers 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/.gitignore: -------------------------------------------------------------------------------- 1 | src/rgbd_assembler/srv/ 2 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | rosbuild_add_executable(rgbd_assembler src/rgbd_assembler_node.cpp) 30 | rosbuild_add_executable(rgbd_assembler_kinect src/rgbd_assembler_kinect_node.cpp) 31 | 32 | rosbuild_add_executable(ping_rgbd_assembler src/ping_rgbd_assembler_node.cpp) 33 | #target_link_libraries(example ${PROJECT_NAME}) 34 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/launch/openni_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/launch/openni_node_new.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/launch/rgbd_assembler.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/launch/sim_rgbd_assembler.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b rgbd_assembler is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Upon a request, this service collects the necessary 5 | sensory data to transfer the colour information from 6 | the wide field to the narrow field camera image. 7 | 8 | Since the narrow field cameras of the PR2 are monochrome, 9 | this can be useful for any algorithm that does, e.g., 10 | segmentation or recognition based partially on colour information. 11 | 12 | 13 | Jeannette Bohg 14 | BSD 15 | 16 | http://ros.org/wiki/rgbd_assembler 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/src/rgbd_assembler/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/manipulation/rgbd_assembler/src/rgbd_assembler/__init__.py -------------------------------------------------------------------------------- /manipulation/rgbd_assembler/srv/RgbdAssembly.srv: -------------------------------------------------------------------------------- 1 | # empty request, as the node listens for incoming sensor messages on its own 2 | 3 | --- 4 | 5 | # The point cloud reconstructed from narrow stereo, 6 | # containing rgb information from wide field and 7 | # pixel coordinates of narrow stereo in wide field 8 | sensor_msgs/PointCloud2 point_cloud 9 | 10 | # narrow stereo image 11 | sensor_msgs/Image image 12 | 13 | # narrow stereo disparity image 14 | stereo_msgs/DisparityImage disparity_image 15 | 16 | # narrow stereo camera information 17 | sensor_msgs/CameraInfo camera_info 18 | 19 | # Whether the assembly has succeeded or failed 20 | int32 OTHER_ERROR = 1 21 | int32 SUCCESS = 2 22 | int32 result -------------------------------------------------------------------------------- /manipulation/segmented_clutter_grasp_planner/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /manipulation/segmented_clutter_grasp_planner/launch/segmented_clutter_grasp_planner_server.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /manipulation/segmented_clutter_grasp_planner/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b segmented_clutter_grasp_planner is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /manipulation/segmented_clutter_grasp_planner/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | segmented_clutter_grasp_planner: segments clutter into clusters by removing points whose normals are parallel to a given grasp direction, and then running the point cluster grasp planner on the resulting clusters 5 | 6 | 7 | Kaijen Hsiao 8 | BSD 9 | 10 | http://ros.org/wiki/segmented_clutter_grasp_planner 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /manipulation/segmented_clutter_grasp_planner/src/segmented_clutter_grasp_planner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-interactive-manipulation/pr2_object_manipulation/1ab5d0f131a891d1ee84b0d460bd1ca9d962453c/manipulation/segmented_clutter_grasp_planner/src/segmented_clutter_grasp_planner/__init__.py -------------------------------------------------------------------------------- /navigation/pr2_navigation_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | rosbuild_add_library(goal_passer src/goal_passer.cpp manifest.xml planner_plugins.xml) 25 | rosbuild_add_library(pose_follower src/pose_follower.cpp manifest.xml ) 26 | 27 | #common commands for building c++ executables and libraries 28 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 29 | #target_link_libraries(${PROJECT_NAME} another_library) 30 | #rosbuild_add_boost_directories() 31 | #rosbuild_link_boost(${PROJECT_NAME} thread) 32 | #rosbuild_add_executable(example examples/example.cpp) 33 | #target_link_libraries(example ${PROJECT_NAME}) 34 | -------------------------------------------------------------------------------- /navigation/pr2_navigation_controllers/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /navigation/pr2_navigation_controllers/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pr2_navigation_controllers: Some base controllers pulled in from navigation_experimental so that we can use them for manipulation. 5 | 6 | 7 | Ported by Adam Leeper, originally by Eitan Marder-Eppstein 8 | BSD 9 | 10 | http://ros.org/wiki/pr2_navigation_controllers 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /navigation/pr2_navigation_controllers/planner_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | Just passes on a goal to the local planner 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | A implementation of a local planner that attempts to follow a plan as closely as possible. 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/.gitignore: -------------------------------------------------------------------------------- 1 | src 2 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | #generate the actions 13 | rosbuild_find_ros_package(actionlib_msgs) 14 | include(${actionlib_msgs_PACKAGE_PATH}/cmake/actionbuild.cmake) 15 | genaction() 16 | 17 | rosbuild_init() 18 | 19 | #set the default path for built executables to the "bin" directory 20 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 21 | #set the default path for built libraries to the "lib" directory 22 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 23 | 24 | rosbuild_genmsg() 25 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/action/ObjectRecognitionGui.action: -------------------------------------------------------------------------------- 1 | 2 | #the original sensor data (depth/disparity image) 3 | sensor_msgs/Image image 4 | sensor_msgs/CameraInfo camera_info 5 | 6 | #list of mesh/pose hypotheses for each recognized point cluster 7 | ModelHypothesisList[] model_hypotheses 8 | --- 9 | 10 | #the index of the model hypothesis that the user has selected for each cluster 11 | #values below 0 mean 'reject all hypotheses' 12 | int32[] selected_hypothesis_indices 13 | --- 14 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/action/ObjectSegmentationGui.action: -------------------------------------------------------------------------------- 1 | sensor_msgs/Image image 2 | sensor_msgs/CameraInfo camera_info 3 | sensor_msgs/Image wide_field 4 | sensor_msgs/CameraInfo wide_camera_info 5 | 6 | sensor_msgs/PointCloud2 point_cloud 7 | stereo_msgs/DisparityImage disparity_image 8 | 9 | --- 10 | # The information for the plane that has been detected 11 | tabletop_object_detector/Table table 12 | 13 | # The raw clusters detected in the scan 14 | sensor_msgs/PointCloud[] clusters 15 | 16 | # Whether the detection has succeeded or failed 17 | int32 NO_CLOUD_RECEIVED = 1 18 | int32 NO_TABLE = 2 19 | int32 OTHER_ERROR = 3 20 | int32 SUCCESS = 4 21 | int32 result 22 | 23 | --- 24 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b interactive_perception_actions 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | interactive_perception_actions 5 | 6 | 7 | jbinney 8 | BSD 9 | 10 | http://ros.org/wiki/interactive_perception_actions 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/msg/.gitignore: -------------------------------------------------------------------------------- 1 | ObjectRecognitionGuiActionFeedback.msg 2 | ObjectRecognitionGuiActionGoal.msg 3 | ObjectRecognitionGuiAction.msg 4 | ObjectRecognitionGuiActionResult.msg 5 | ObjectRecognitionGuiFeedback.msg 6 | ObjectRecognitionGuiGoal.msg 7 | ObjectRecognitionGuiResult.msg 8 | ObjectSegmentationGuiActionFeedback.msg 9 | ObjectSegmentationGuiActionGoal.msg 10 | ObjectSegmentationGuiAction.msg 11 | ObjectSegmentationGuiActionResult.msg 12 | ObjectSegmentationGuiFeedback.msg 13 | ObjectSegmentationGuiGoal.msg 14 | ObjectSegmentationGuiResult.msg 15 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/msg/ModelHypothesis.msg: -------------------------------------------------------------------------------- 1 | #describes a hypothesis about a recognized object (mesh+pose) 2 | 3 | shape_msgs/Mesh mesh 4 | geometry_msgs/PoseStamped pose 5 | -------------------------------------------------------------------------------- /perception/interactive_perception_msgs/msg/ModelHypothesisList.msg: -------------------------------------------------------------------------------- 1 | ModelHypothesis[] hypotheses 2 | 3 | #initial guess if this can be a correct recognition result at all 4 | bool accept -------------------------------------------------------------------------------- /perception/object_recognition_gui/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | # CMake 2.4.6 doesn't include FindPkgConfig, so we provide our own copy 4 | include($ENV{ROS_ROOT}/core/rosbuild/FindPkgConfig.cmake) 5 | 6 | # Set the build type. Options are: 7 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 8 | # Debug : w/ debug symbols, w/o optimization 9 | # Release : w/o debug symbols, w/ optimization 10 | # RelWithDebInfo : w/ debug symbols, w/ optimization 11 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 12 | #set(ROS_BUILD_TYPE RelWithDebInfo) 13 | 14 | rosbuild_init() 15 | 16 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | # build is where the UI file outputs (ui_*.h) go. 20 | include_directories( build ) 21 | 22 | find_package(Qt4 COMPONENTS QtCore QtGui QtOpenGL REQUIRED) 23 | include(${QT_USE_FILE}) 24 | add_definitions(-DQT_NO_KEYWORDS) 25 | 26 | qt4_wrap_ui(UIC_FILES 27 | src/object_recognition_frame.ui 28 | ) 29 | 30 | qt4_wrap_cpp(MOC_FILES 31 | include/object_recognition_gui/object_recognition_rviz_ui.h 32 | include/object_recognition_gui/mouse_event_signalling_render_panel.h 33 | ) 34 | 35 | rosbuild_add_library(object_recognition_gui_rviz_plugin 36 | src/init.cpp 37 | src/object_recognition_display.cpp 38 | src/object_recognition_rviz_ui.cpp 39 | src/mouse_event_signalling_render_panel.cpp 40 | ${UIC_FILES} 41 | ${MOC_FILES} 42 | ) 43 | 44 | target_link_libraries(object_recognition_gui_rviz_plugin ${QT_LIBRARIES}) 45 | 46 | rosbuild_add_executable(ping_object_recognition_gui src/ping_object_recognition_gui.cpp) -------------------------------------------------------------------------------- /perception/object_recognition_gui/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /perception/object_recognition_gui/lib/rviz_plugin.yaml: -------------------------------------------------------------------------------- 1 | name: Object Recognition GUI 2 | library: object_recognition_gui_rviz_plugin 3 | displays: 4 | - 5 | class_name: object_recognition_gui::ObjectRecognitionDisplay 6 | display_name: Object Recognition Pop-Up Window 7 | description: | 8 | Creates a popup window in case the robot needs assistance with 9 | the recognition of objects. It enables the user is able to 10 | correct the recognition results. 11 | -------------------------------------------------------------------------------- /perception/object_recognition_gui/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b pr2_gripper_click is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /perception/object_recognition_gui/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A package for user-aided object detection and pose estimation 5 | on pre-segmented point clouds. 6 | 7 | 8 | David Gossow 9 | BSD 10 | 11 | http://ros.org/wiki/object_detection_gui 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /perception/object_recognition_gui/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Creates a popup window in case the robot needs assistance with 7 | the recognition of objects. The user is able to 8 | correct the recognition results. 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /perception/object_recognition_gui/src/object_recognition_gui/__init__.py: -------------------------------------------------------------------------------- 1 | #autogenerated by ROS python message generators -------------------------------------------------------------------------------- /perception/robot_self_filter_color/.gitignore: -------------------------------------------------------------------------------- 1 | self_filter_color 2 | test_filter_color 3 | -------------------------------------------------------------------------------- /perception/robot_self_filter_color/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /perception/robot_self_filter_color/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b robot_self_filter_color is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /perception/robot_self_filter_color/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | The robot self filter is a package that allows you to filter out robot body parts from sensor information using the known positions and orientations of these parts. (same as robot_self_filter in arm_navigation, but with PointXYZRGB.) 4 | 5 | Ioan Sucan 6 | BSD 7 | 8 | http://ros.org/wiki/robot_self_filter_color 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /perception/tabletop_collision_map_processing/.gitignore: -------------------------------------------------------------------------------- 1 | src/tabletop_collision_map_processing/srv/ 2 | -------------------------------------------------------------------------------- /perception/tabletop_collision_map_processing/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | 32 | rosbuild_add_library(${PROJECT_NAME} src/collision_map_interface.cpp) 33 | 34 | rosbuild_add_executable(tabletop_collision_map_processing_node nodes/tabletop_collision_map_processing_node.cpp) 35 | target_link_libraries(tabletop_collision_map_processing_node ${PROJECT_NAME}) -------------------------------------------------------------------------------- /perception/tabletop_collision_map_processing/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /perception/tabletop_collision_map_processing/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b tabletop_collision_processing is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /perception/tabletop_collision_map_processing/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Populates and manages a collision environment using the results of 5 | sensor processing from the tabletop_object_detector. Functionality 6 | includes adding and removing detected objects from the collision 7 | environment, and requesting static collision maps for obstacle 8 | avoidance. 9 | 10 | 11 | Matei Ciocarlie 12 | BSD 13 | 14 | http://ros.org/wiki/tabletop_collision_map_processing 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /perception/tabletop_collision_map_processing/src/tabletop_collision_map_processing/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /perception/tabletop_collision_map_processing/srv/TabletopCollisionMapProcessing.srv: -------------------------------------------------------------------------------- 1 | # given the result of a tabletop detection, adds the objects to 2 | # the collision map and returns them as instances of GraspableObjects 3 | # with correct collision names 4 | 5 | # a list of clusters and database models, directly as produced by the detector 6 | # note that it is possible to not set the Table inside here (leave its 7 | # extents set to zero), in which case it does not get added to the environment 8 | tabletop_object_detector/TabletopDetectionResult detection_result 9 | 10 | # whether the current collision models should be reset before adding new models 11 | bool reset_collision_models 12 | 13 | # whether the current list of models attached to the robot should be reset 14 | bool reset_attached_models 15 | 16 | # what tf frame the results should be returned in 17 | # if empty, results will be in the same frame as the input 18 | string desired_frame 19 | 20 | --- 21 | 22 | # the objects added to the collision environment and packaged as graspable objects 23 | manipulation_msgs/GraspableObject[] graspable_objects 24 | 25 | # the list of collision names that the objects got in the environment 26 | # in the same order as graspable_objects 27 | string[] collision_object_names 28 | 29 | # the name that the table got in the collision map, if any 30 | string collision_support_surface_name 31 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/.gitignore: -------------------------------------------------------------------------------- 1 | src/tabletop_object_detector/msg/ 2 | src/tabletop_object_detector/srv/ 3 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /perception/tabletop_object_detector/launch/tabletop_complete.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/launch/tabletop_complete_standalone.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/launch/tabletop_object_recognition.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b Given a point cloud, will attempt to identify and localize known objects. 6 | 7 | Works for detecting objects from the IKEA dataset, assuming they are sitting 8 | on a table, which is the dominant plane in a scan. It will first find the 9 | dominant plane, then cluster objects based on the projections of the points 10 | on the plane. For each cluster, it will then attempt to fit the models 11 | of the IKEA objects. Models are assumed to be upright on the table and 12 | rotationally symmetric. 13 | 14 | Functionality is broken down into two main parts: clustering and model fitting. 15 | 16 | 23 | 24 | \section codeapi Code API 25 | 26 | 36 | 37 | 38 | */ -------------------------------------------------------------------------------- /perception/tabletop_object_detector/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Performs object segmentation and simple object recognition for 4 | constrained scenes. 5 | 6 | 7 | Marius Muja and Matei Ciocarlie 8 | BSD 9 | 10 | http://ros.org/wiki/object_detector 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/msg/Table.msg: -------------------------------------------------------------------------------- 1 | # Informs that a planar table has been detected at a given location 2 | 3 | # The pose gives you the transform that take you to the coordinate system 4 | # of the table, with the origin somewhere in the table plane and the 5 | # z axis normal to the plane 6 | geometry_msgs/PoseStamped pose 7 | 8 | # These values give you the observed extents of the table, along x and y, 9 | # in the table's own coordinate system (above) 10 | # there is no guarantee that the origin of the table coordinate system is 11 | # inside the boundary defined by these values. 12 | float32 x_min 13 | float32 x_max 14 | float32 y_min 15 | float32 y_max 16 | 17 | # There is no guarantee that the table does NOT extend further than these 18 | # values; this is just as far as we've observed it. 19 | 20 | 21 | # Newer table definition as triangle mesh of convex hull (relative to pose) 22 | shape_msgs/Mesh convex_hull 23 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/msg/TabletopDetectionResult.msg: -------------------------------------------------------------------------------- 1 | # Contains all the information from one run of the tabletop detection node 2 | 3 | # The information for the plane that has been detected 4 | Table table 5 | 6 | # The raw clusters detected in the scan 7 | sensor_msgs/PointCloud[] clusters 8 | 9 | # The list of potential models that have been detected for each cluster 10 | # An empty list will be returned for a cluster that has no recognition results at all 11 | household_objects_database_msgs/DatabaseModelPoseList[] models 12 | 13 | # For each cluster, the index of the list of models that was fit to that cluster 14 | # keep in mind that multiple raw clusters can correspond to a single fit 15 | int32[] cluster_model_indices 16 | 17 | # Whether the detection has succeeded or failed 18 | int32 NO_CLOUD_RECEIVED = 1 19 | int32 NO_TABLE = 2 20 | int32 OTHER_ERROR = 3 21 | int32 SUCCESS = 4 22 | int32 result 23 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/src/tabletop_object_detector/__init__.py: -------------------------------------------------------------------------------- 1 | #autogenerated by ROS python message generators -------------------------------------------------------------------------------- /perception/tabletop_object_detector/srv/AddModelExclusion.srv: -------------------------------------------------------------------------------- 1 | # The model to exclude 2 | int32 model_id 3 | --- 4 | #no reply 5 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/srv/ClearExclusionsList.srv: -------------------------------------------------------------------------------- 1 | #empty 2 | --- 3 | #no reply 4 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/srv/NegateExclusions.srv: -------------------------------------------------------------------------------- 1 | bool negate 2 | --- 3 | #no reply 4 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/srv/SegmentObjectInHand.srv: -------------------------------------------------------------------------------- 1 | # The name of the wrist frame for the arm in question (the region of interest in the params are relative to this) 2 | string wrist_frame 3 | 4 | --- 5 | 6 | # The resulting cluster of points in the hand 7 | sensor_msgs/PointCloud2 cluster 8 | 9 | # Whether the segmentation succeeded or failed 10 | int32 SUCCESS = 0 11 | int32 NO_CLOUD_RECEIVED = 1 12 | int32 TF_ERROR = 2 13 | int32 OTHER_ERROR = 3 14 | int32 result 15 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/srv/TabletopDetection.srv: -------------------------------------------------------------------------------- 1 | # service will run on last received cloud, no cloud is passed in 2 | 3 | # clustering is done as an intermediate step for model detection 4 | # but we can also return the clusters themselves if the caller wants them 5 | # this flag tells whether the computed clusters should be returned 6 | bool return_clusters 7 | 8 | # if only clusters are of interest, then model fitting can be skipped 9 | # this flag tells whether model fitting should be performed and the results returned 10 | bool return_models 11 | 12 | # The number of potential fit models which should be returned for each cluster 13 | int32 num_models 14 | --- 15 | # reply is message with table and models 16 | TabletopDetectionResult detection 17 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/srv/TabletopObjectRecognition.srv: -------------------------------------------------------------------------------- 1 | # The information for the plane that has been detected 2 | Table table 3 | 4 | # The raw clusters detected in the scan 5 | sensor_msgs/PointCloud[] clusters 6 | 7 | # How many models should be returned for each recognition 8 | int32 num_models 9 | 10 | # Whether to perform a merge step based on model fit results 11 | bool perform_fit_merge 12 | 13 | --- 14 | 15 | # The list of models that have been detected along with confidence levels 16 | household_objects_database_msgs/DatabaseModelPoseList[] models 17 | 18 | # For each cluster, the index of the list of models that was fit to that cluster 19 | # keep in mind that multiple raw clusters can correspond to a single fit 20 | int32[] cluster_model_indices 21 | -------------------------------------------------------------------------------- /perception/tabletop_object_detector/srv/TabletopSegmentation.srv: -------------------------------------------------------------------------------- 1 | # typical use doesn't put anything in the request, as the node listens for incoming point clouds on its own 2 | 3 | # optional table if you want to re-use a previously-detected table instead of re-detecting (leave empty to detect table) 4 | Table table 5 | 6 | --- 7 | 8 | # The information for the plane that has been detected 9 | Table table 10 | 11 | # The raw clusters detected in the scan 12 | sensor_msgs/PointCloud[] clusters 13 | 14 | # Whether the detection has succeeded or failed 15 | int32 NO_CLOUD_RECEIVED = 1 16 | int32 NO_TABLE = 2 17 | int32 OTHER_ERROR = 3 18 | int32 SUCCESS = 4 19 | int32 result 20 | -------------------------------------------------------------------------------- /rosdep.yaml: -------------------------------------------------------------------------------- 1 | psycopg2: 2 | ubuntu: python-psycopg2 3 | 4 | python-vtk: 5 | ubuntu: python-vtk 6 | --------------------------------------------------------------------------------