From 358ac117ba7d75fff48ceb42d059862bec778cdc Mon Sep 17 00:00:00 2001 From: RyuYamamoto Date: Tue, 19 May 2015 17:39:14 +0900 Subject: [PATCH 1/2] enable octomap --- .../config/kinect_sensor.yaml | 8 ++++ .../NextageOpen_moveit_sensor_manager.launch | 40 +++++++++++++++++++ .../launch/sensor_manager.launch | 16 ++++---- 3 files changed, 57 insertions(+), 7 deletions(-) create mode 100644 nextage_moveit_config/config/kinect_sensor.yaml diff --git a/nextage_moveit_config/config/kinect_sensor.yaml b/nextage_moveit_config/config/kinect_sensor.yaml new file mode 100644 index 00000000..c6025437 --- /dev/null +++ b/nextage_moveit_config/config/kinect_sensor.yaml @@ -0,0 +1,8 @@ +sensors: +- sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater +point_cloud_topic: /camera/depth/points +max_range: 5.0 +point_subsample: 1 +padding_offset: 0.1 +padding_scale: 1.0 +filtered_cloud_topic: filtered_cloud diff --git a/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch b/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch index 5d02698d..41d8e41f 100644 --- a/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch +++ b/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch @@ -1,3 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_moveit_config/launch/sensor_manager.launch b/nextage_moveit_config/launch/sensor_manager.launch index 9848ced9..05a025b1 100644 --- a/nextage_moveit_config/launch/sensor_manager.launch +++ b/nextage_moveit_config/launch/sensor_manager.launch @@ -1,9 +1,11 @@ - - - - - - - + + + + + + + + + From 7f181c3562806f40d14a599f860bc54a58a9cce1 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 2/2] Add kinect-checkerboard capability --- .../config/kinect_sensor.yaml | 2 +- .../NextageOpen_moveit_sensor_manager.launch | 43 +- .../launch/move_group.launch | 3 + .../launch/moveit_kinect.rviz | 368 ++++++++++++++++++ .../launch/moveit_planning_execution.launch | 3 + .../launch/sensor_manager.launch | 2 - .../launch/kinect_checkerboard.launch | 41 ++ nextage_ros_bridge/package.xml | 1 + .../script/objectdetection_tf_publisher.py | 78 ++++ 9 files changed, 499 insertions(+), 42 deletions(-) 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/config/kinect_sensor.yaml b/nextage_moveit_config/config/kinect_sensor.yaml index c6025437..fdc5394e 100644 --- a/nextage_moveit_config/config/kinect_sensor.yaml +++ b/nextage_moveit_config/config/kinect_sensor.yaml @@ -1,6 +1,6 @@ sensors: - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater -point_cloud_topic: /camera/depth/points +point_cloud_topic: /camera/depth_registered/points max_range: 5.0 point_subsample: 1 padding_offset: 0.1 diff --git a/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch b/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch index 41d8e41f..af5c641e 100644 --- a/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch +++ b/nextage_moveit_config/launch/NextageOpen_moveit_sensor_manager.launch @@ -1,43 +1,8 @@ - - - - - - - - - - - - - - - - + + + + - - - - - - - - - - - - - - - - - - - - - diff --git a/nextage_moveit_config/launch/move_group.launch b/nextage_moveit_config/launch/move_group.launch index 9b83d8e1..65c6ecb0 100644 --- a/nextage_moveit_config/launch/move_group.launch +++ b/nextage_moveit_config/launch/move_group.launch @@ -15,6 +15,8 @@ + + @@ -26,6 +28,7 @@ + diff --git a/nextage_moveit_config/launch/moveit_kinect.rviz b/nextage_moveit_config/launch/moveit_kinect.rviz new file mode 100644 index 00000000..8d594df0 --- /dev/null +++ b/nextage_moveit_config/launch/moveit_kinect.rviz @@ -0,0 +1,368 @@ +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 + - 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_registered/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 + Target Frame: + Value: Orbit (rviz) + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 811 + Hide Left Dock: false + Hide Right Dock: false + Motion Planning: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1573 diff --git a/nextage_moveit_config/launch/moveit_planning_execution.launch b/nextage_moveit_config/launch/moveit_planning_execution.launch index fd579331..c580abdf 100644 --- a/nextage_moveit_config/launch/moveit_planning_execution.launch +++ b/nextage_moveit_config/launch/moveit_planning_execution.launch @@ -1,8 +1,11 @@ + # The planning and execution components of MoveIt! configured to # publish the current configuration of the robot (simulated or real) # and the current state of the world as seen by the planner. + + # The visualization component of MoveIt! diff --git a/nextage_moveit_config/launch/sensor_manager.launch b/nextage_moveit_config/launch/sensor_manager.launch index 05a025b1..001f59d0 100644 --- a/nextage_moveit_config/launch/sensor_manager.launch +++ b/nextage_moveit_config/launch/sensor_manager.launch @@ -5,7 +5,5 @@ - - diff --git a/nextage_ros_bridge/launch/kinect_checkerboard.launch b/nextage_ros_bridge/launch/kinect_checkerboard.launch new file mode 100644 index 00000000..e0c65201 --- /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()