From bebb6f398c9a8b317893c22f3a7bd929a188fd35 Mon Sep 17 00:00:00 2001 From: Isaac IY Saito <130s@lateeye.net> Date: Fri, 15 May 2015 19:28:23 +0900 Subject: [PATCH] Add kinect-checkerboard capability --- .../launch/moveit_kinect.rviz | 374 ++++++++++++++++++ .../launch/kinect_checkerboard.launch | 41 ++ nextage_ros_bridge/package.xml | 1 + .../script/objectdetection_tf_publisher.py | 78 ++++ 4 files changed, 494 insertions(+) create mode 100644 nextage_moveit_config/launch/moveit_kinect.rviz create mode 100644 nextage_ros_bridge/launch/kinect_checkerboard.launch create mode 100755 nextage_ros_bridge/script/objectdetection_tf_publisher.py diff --git a/nextage_moveit_config/launch/moveit_kinect.rviz b/nextage_moveit_config/launch/moveit_kinect.rviz new file mode 100644 index 00000000..c1ada687 --- /dev/null +++ b/nextage_moveit_config/launch/moveit_kinect.rviz @@ -0,0 +1,374 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MotionPlanning1/Planning Request1 + - /PointCloud21 + - /PointCloud21/Status1 + Splitter Ratio: 0.5 + Tree Height: 97 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + Name: MotionPlanning + Planned Path: + Links: + All Links Enabled: true + CHEST_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + HEAD_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + HEAD_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT3_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT4_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT5_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RARM_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT3_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT4_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT5_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WAIST: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trajectory Topic: /move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.08 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: right_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.9 + Scene Color: 50; 230; 50 + Scene Display Time: 0.2 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + CHEST_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + HEAD_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + HEAD_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT3_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT4_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT5_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RARM_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT3_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT4_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT5_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WAIST: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /collision_detector_marker_array + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: "" + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /camera/depth/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: WAIST + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.08231 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.0312451 + Y: 0.113699 + Z: 0.380208 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.490398 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.79358 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 811 + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000002ba000002a1fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000000f0000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e0067010000011e000001ab000001ab00ffffff000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006250000003efc0100000002fb0000000800540069006d0065010000000000000625000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000365000002a100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1573 + X: 51 + Y: 21 diff --git a/nextage_ros_bridge/launch/kinect_checkerboard.launch b/nextage_ros_bridge/launch/kinect_checkerboard.launch new file mode 100644 index 00000000..f53a422b --- /dev/null +++ b/nextage_ros_bridge/launch/kinect_checkerboard.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_ros_bridge/package.xml b/nextage_ros_bridge/package.xml index bf1f608a..4eb48e1d 100644 --- a/nextage_ros_bridge/package.xml +++ b/nextage_ros_bridge/package.xml @@ -21,6 +21,7 @@ nextage_description roslint + freenect_launch hironx_ros_bridge nextage_description ueye_cam diff --git a/nextage_ros_bridge/script/objectdetection_tf_publisher.py b/nextage_ros_bridge/script/objectdetection_tf_publisher.py new file mode 100755 index 00000000..9aae66c3 --- /dev/null +++ b/nextage_ros_bridge/script/objectdetection_tf_publisher.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python +import rospy +import roslib +roslib.load_manifest("posedetection_msgs") +roslib.load_manifest("dynamic_tf_publisher") +from posedetection_msgs.msg import ObjectDetection +from geometry_msgs.msg import Transform +from dynamic_tf_publisher.srv import * + +class ObjectDetectionTfPublisher(): + def __init__(self): + self.subscriber = rospy.Subscriber("ObjectDetection", ObjectDetection, self.callback); + self.frame_id = rospy.get_param("~frame_id", "object") + self.init_object_messages() + + def init_object_messages(self): + if rospy.has_param('~checker_board_params/position_x'): + position_x = rospy.get_param('~checker_board_params/position_x', 0) + position_y = rospy.get_param('~checker_board_params/position_y', 0) + position_z = rospy.get_param('~checker_board_params/position_z', 0) + orientation_x = rospy.get_param('~checker_board_params/orientation_x', 0) + orientation_y = rospy.get_param('~checker_board_params/orientation_y', 0) + orientation_z = rospy.get_param('~checker_board_params/orientation_z', 0) + orientation_w = rospy.get_param('~checker_board_params/orientation_w', 0) + rospy.loginfo("objectdetection_tf_publisher load the calibration params :") + rospy.loginfo(" pos : %f %f %f orientation %f %f %f %f", position_x, position_y, position_z, orientation_x, orientation_y, orientation_z, orientation_w) + + self.send_dynamic_tf_request(position_x, position_y, position_z, + orientation_x, orientation_y, orientation_z, orientation_w, + rospy.get_param('~checker_board_params/header_frame'), + self.frame_id + ) + else: + rospy.loginfo("No parameters (~checker_board_params) was found for object detection") + + def callback(self, msg): + if msg.objects: + for detected_object in msg.objects: + self.send_dynamic_tf_request( + detected_object.pose.position.x, + detected_object.pose.position.y, + detected_object.pose.position.z, + detected_object.pose.orientation.x, + detected_object.pose.orientation.y, + detected_object.pose.orientation.z, + detected_object.pose.orientation.w, + msg.header.frame_id, + self.frame_id + ) + + def send_dynamic_tf_request(self, pos_x, pos_y, pos_z, ori_x, ori_y, ori_z, ori_w, parent_frame_id, child_frame_id): + transform = Transform() + transform.translation.x = pos_x + transform.translation.y = pos_y + transform.translation.z = pos_z + transform.rotation.x = ori_x + transform.rotation.y = ori_y + transform.rotation.z = ori_z + transform.rotation.w = ori_w + + set_tf_request = SetDynamicTFRequest() + set_tf_request.freq = 10 + set_tf_request.cur_tf.header.stamp = rospy.get_rostime() + set_tf_request.cur_tf.header.frame_id = parent_frame_id + set_tf_request.cur_tf.child_frame_id = child_frame_id + set_tf_request.cur_tf.transform = transform + + rospy.wait_for_service('/set_dynamic_tf') + set_dynamic_tf = rospy.ServiceProxy('/set_dynamic_tf', SetDynamicTF) + try: + res = set_dynamic_tf(set_tf_request) + except rospy.ServiceException as exc: + print("Service did not process request: " + str(exc)) + +if __name__== '__main__': + rospy.init_node('objectdetection_tf_publisher', anonymous=True) + object_detection_tf_publisher = ObjectDetectionTfPublisher() + rospy.spin()