diff --git a/.flake8 b/.flake8 new file mode 100644 index 0000000..cdffbdc --- /dev/null +++ b/.flake8 @@ -0,0 +1,10 @@ +[flake8] +# The line length here has to match the black config in pyproject.toml +max-line-length = 120 +exclude = + .git, + __pycache__ +extend-ignore = + # See https://github.com/PyCQA/pycodestyle/issues/373 + E203, + E741, diff --git a/.mdl_style.rb b/.mdl_style.rb new file mode 100644 index 0000000..8d48b76 --- /dev/null +++ b/.mdl_style.rb @@ -0,0 +1,26 @@ +# Style file for markdownlint + +all + +exclude_rule 'fenced-code-language' # Fenced code blocks should have a language specified +exclude_rule 'first-line-h1' # First line in file should be a top level header + +exclude_rule 'ul-style' + +exclude_rule 'no-multiple-blanks' + +exclude_rule 'header-style' + +exclude_rule 'no-bare-urls' + +# Line lenght +rule 'MD013', :line_length => 120, :code_blocks => false, :tables => false + +# Unordered list indentation +rule 'MD007', :indent => 2 + +# Ordered list item prefix +rule 'MD029', :style => 'ordered' + +# Multiple headers with the same content +rule 'no-duplicate-header', :allow_different_nesting => true diff --git a/.mdlrc b/.mdlrc new file mode 100644 index 0000000..71eadf9 --- /dev/null +++ b/.mdlrc @@ -0,0 +1,2 @@ +git_recurse true +style ".mdl_style.rb" diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..5aef944 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,73 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.1.0 + hooks: + - id: check-added-large-files + - id: check-case-conflict + - id: check-executables-have-shebangs + - id: check-docstring-first + - id: check-merge-conflict + - id: check-shebang-scripts-are-executable + - id: check-symlinks + - id: check-vcs-permalinks + - id: check-xml + - id: check-yaml + exclude: mobipick_description/config/joint_limits.yaml + - id: debug-statements + - id: end-of-file-fixer + exclude: &excludes | + (?x)^( + .*\.blend| + .*\.dae| + .*\.mtl| + .*\.obj| + .*\.pgm| + .*\.step| + .*\.stl + )$ + - id: fix-byte-order-marker + - id: mixed-line-ending + exclude: *excludes + - id: trailing-whitespace + exclude: *excludes + + - repo: https://github.com/psf/black + rev: 22.3.0 + hooks: + - id: black + + - repo: https://github.com/PyCQA/flake8.git + rev: 5.0.4 + hooks: + - id: flake8 + + - repo: https://github.com/markdownlint/markdownlint + rev: v0.11.0 + hooks: + - id: markdownlint + + - repo: local + hooks: + - id: catkin_lint + name: catkin_lint + description: Check package.xml and cmake files + entry: catkin_lint . + language: system + always_run: true + pass_filenames: false + args: [ "--strict", "--ignore", "UNINSTALLED_SCRIPT" ] diff --git a/CMakeLists.txt b/CMakeLists.txt index 2147af0..bf2c40f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,17 +20,17 @@ catkin_python_setup() ## Generate actions in the 'action' folder add_action_files( FILES - PickObject.action - PlaceObject.action - InsertObject.action + InsertObject.action + PickObject.action + PlaceObject.action ) ## Generate added messages and services with any dependencies listed here generate_messages( DEPENDENCIES - actionlib_msgs - geometry_msgs - std_msgs + actionlib_msgs + geometry_msgs + std_msgs ) generate_dynamic_reconfigure_options( @@ -43,7 +43,41 @@ generate_dynamic_reconfigure_options( catkin_package( CATKIN_DEPENDS actionlib_msgs + dynamic_reconfigure geometry_msgs message_runtime std_msgs ) + +############# +## Install ## +############# + +catkin_install_python(PROGRAMS + scripts/insert_obj_test_action_client + scripts/object_recognition_mockup.py + scripts/pick_obj_test_action_client + scripts/place_obj_test_action_client + scripts/publish_tf_world_to_robot + scripts/rqt_grasplan + scripts/rqt_planning_scene + scripts/set_rviz_logger_level.py + scripts/visualize_planning_scene_node + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +# Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY + config + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(FILES + plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############# +## Testing ## +############# + +roslaunch_add_file_check(launch) diff --git a/README.md b/README.md index 2977cb8..18b4cac 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,26 @@ # grasplan Documentation can be found under: https://grasplan-documentation.readthedocs.io/en/latest/ + + +pre-commit Formatting Checks +---------------------------- + +This repo has a [pre-commit](https://pre-commit.com/) check that runs in CI. +You can use this locally and set it up to run automatically before you commit +something. To install, use pip: + +```bash +pip3 install --user pre-commit +``` + +To run over all the files in the repo manually: + +```bash +pre-commit run -a +``` + +To run pre-commit automatically before committing in the local repo, install the git hooks: + +```bash +pre-commit install diff --git a/config/object_recognition_mockup/objBounds.cfg b/config/object_recognition_mockup/objBounds.cfg old mode 100644 new mode 100755 diff --git a/launch/grasp_planning/pick_handcoded_grasp_planner.launch b/launch/grasp_planning/pick_handcoded_grasp_planner.launch index 95bd5b1..db0df7a 100644 --- a/launch/grasp_planning/pick_handcoded_grasp_planner.launch +++ b/launch/grasp_planning/pick_handcoded_grasp_planner.launch @@ -5,8 +5,8 @@ - - + + diff --git a/launch/grasp_planning/pick_simple_pregrasp_planner.launch b/launch/grasp_planning/pick_simple_pregrasp_planner.launch index 20fff86..7c6c045 100644 --- a/launch/grasp_planning/pick_simple_pregrasp_planner.launch +++ b/launch/grasp_planning/pick_simple_pregrasp_planner.launch @@ -5,8 +5,8 @@ - - + + diff --git a/launch/visualization/rviz_gripper_visualizer.launch b/launch/visualization/rviz_gripper_visualizer.launch index 6eb2953..16f931c 100644 --- a/launch/visualization/rviz_gripper_visualizer.launch +++ b/launch/visualization/rviz_gripper_visualizer.launch @@ -3,7 +3,7 @@ - + diff --git a/package.xml b/package.xml index 742e751..cef9ecd 100644 --- a/package.xml +++ b/package.xml @@ -8,35 +8,49 @@ MIT - Oscar Lima + Oscar Lima + actionlib_msgs + dynamic_reconfigure + geometry_msgs rqt_gui rqt_gui_py - dynamic_reconfigure + std_msgs actionlib_msgs + dynamic_reconfigure geometry_msgs message_generation + rqt_gui + rqt_gui_py std_msgs actionlib actionlib_msgs dynamic_reconfigure + gazebo_ros geometry_msgs message_runtime + mobipick_description + mobipick_gazebo + mobipick_pick_n_place moveit_commander moveit_msgs + rostopic rqt_gui rqt_gui_py + rviz + spacenav_node std_msgs std_srvs + tables_demo_bringup tf trajectory_msgs + xacro catkin - + - diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..d374d61 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,4 @@ +[tool.black] +line-length = 120 +target-version = ['py38'] +skip-string-normalization = true diff --git a/scripts/insert_obj_test_action_client b/scripts/insert_obj_test_action_client index e4e5318..a1f008a 100755 --- a/scripts/insert_obj_test_action_client +++ b/scripts/insert_obj_test_action_client @@ -22,8 +22,10 @@ if __name__ == '__main__': elif myargv[2] == 'false' or myargv[2] == 'False': observe_before_insert = False else: - rospy.logwarn('you have run this node without arguments, at least the mobipick namespace is expected! ( __ns:=mobipick )') - timeout = 50.0 # in seconds, default: 50.0 + rospy.logwarn( + 'you have run this node without arguments, at least the mobipick namespace is expected! ( __ns:=mobipick )' + ) + timeout = 50.0 # in seconds, default: 50.0 insert_object_server_name = 'insert_object' action_client = actionlib.SimpleActionClient(insert_object_server_name, InsertObjectAction) rospy.loginfo(f'waiting for {insert_object_server_name} action server') @@ -38,11 +40,11 @@ if __name__ == '__main__': if action_client.wait_for_result(rospy.Duration.from_sec(timeout)): result = action_client.get_result() rospy.loginfo(f'{insert_object_server_name} is done with execution, resuĺt was = "{result}"') - if result.success == True: - rospy.loginfo(f'Succesfully inserted object') + if result.success: + rospy.loginfo('Succesfully inserted object') else: - rospy.logerr(f'Failed to insert object') + rospy.logerr('Failed to insert object') else: - rospy.logerr(f'Failed to insert object, timeout?') + rospy.logerr('Failed to insert object, timeout?') else: - rospy.logerr(f'action server {insert_object_server_name} not available') + rospy.logerr('action server {insert_object_server_name} not available') diff --git a/scripts/object_recognition_mockup.py b/scripts/object_recognition_mockup.py index fceb67c..32f4419 100755 --- a/scripts/object_recognition_mockup.py +++ b/scripts/object_recognition_mockup.py @@ -18,15 +18,17 @@ import tf2_ros import tf2_geometry_msgs -''' -Define a virtual box with position and dimensions that represents the field of view of a camera -Query Gazebo for all objects in the scene -Iterate over all objects, see if they are inside the box, if so then publish their pose as cob_perception_msgs/DetectionArray -Publish the object detections as tf -This node can be used as a mockup for object 6D pose estimation and classification -''' class ObjRecognitionMockup: + ''' + Define a virtual box with position and dimensions that represents the field of view of a camera + Query Gazebo for all objects in the scene + Iterate over all objects, see if they are inside the box, if so then publish their pose as + cob_perception_msgs/DetectionArray + Publish the object detections as tf + This node can be used as a mockup for object 6D pose estimation and classification + ''' + def __init__(self, test_pose=False): # get object bounding box from parameter self.bounding_boxes = rospy.get_param('~bounding_boxes', []) @@ -58,7 +60,7 @@ def __init__(self, test_pose=False): self.tf_broadcaster = tf.TransformBroadcaster() # to transform the object pose into another reference frame (self.objects_desired_reference_frame) self.tf_buffer = tf2_ros.Buffer(rospy.Duration(100.0)) # tf buffer length - tf_listener = tf2_ros.TransformListener(self.tf_buffer) + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) # keep track of the most approximate gazebo gt timestamp self.model_states_timestamp = rospy.Time.now() @@ -85,15 +87,16 @@ def transform_pose(self, input_pose, input_pose_reference_frame, target_frame_id try: transform = self.tf_buffer.lookup_transform( - # target reference frame - target_frame_id, - # source frame: - input_pose_reference_frame, - # get the tf at the time the pose was valid - pose_stamped_msg.header.stamp, - # wait for at most 1 second for transform, otherwise throw - rospy.Duration(1.0)) - except: + # target reference frame + target_frame_id, + # source frame: + input_pose_reference_frame, + # get the tf at the time the pose was valid + pose_stamped_msg.header.stamp, + # wait for at most 1 second for transform, otherwise throw + rospy.Duration(1.0), + ) + except tf2_ros.TransformException: rospy.logerr(f'could not transform pose from {input_pose_reference_frame} to {target_frame_id}') return None @@ -125,7 +128,7 @@ def eventInCB(self, msg): detection.pose.pose = copy.deepcopy(pose_msg.pose) # get bounding box from parameter yaml file - object_class = separate_object_class_from_id(model_name)[0] # get object class from anchored object + object_class = separate_object_class_from_id(model_name)[0] # get object class from anchored object if self.bounding_boxes: if object_class in self.bounding_boxes: detection.bounding_box_lwh.x = self.bounding_boxes[object_class]['box_x'] @@ -133,7 +136,10 @@ def eventInCB(self, msg): detection.bounding_box_lwh.z = self.bounding_boxes[object_class]['box_z'] else: if not self.supress_warnings: - rospy.logwarn(f'object_class: {object_class} bounding box not found in parameters, will leave bb empty') + rospy.logwarn( + f'object_class: {object_class} bounding box not found in parameters,' + ' will leave bb empty' + ) detection.bounding_box_lwh.x = 0.0 detection.bounding_box_lwh.y = 0.0 detection.bounding_box_lwh.z = 0.0 @@ -144,9 +150,22 @@ def eventInCB(self, msg): detections_msg.detections.append(detection) if self.broadcast_object_tf: # broadcast object tf - self.tf_broadcaster.sendTransform((detection.pose.pose.position.x, detection.pose.pose.position.y, detection.pose.pose.position.z), - (detection.pose.pose.orientation.x, detection.pose.pose.orientation.y, detection.pose.pose.orientation.z, detection.pose.pose.orientation.w), - detection.pose.header.stamp, self.model_states_msg.name[i], self.objects_desired_reference_frame) + self.tf_broadcaster.sendTransform( + ( + detection.pose.pose.position.x, + detection.pose.pose.position.y, + detection.pose.pose.position.z, + ), + ( + detection.pose.pose.orientation.x, + detection.pose.pose.orientation.y, + detection.pose.pose.orientation.z, + detection.pose.pose.orientation.w, + ), + detection.pose.header.stamp, + self.model_states_msg.name[i], + self.objects_desired_reference_frame, + ) if len(detections_msg.detections) == 0: if not self.supress_warnings: @@ -189,7 +208,7 @@ def publish_perception_fov(self, config): marker_msg.color.r = 0.0 marker_msg.color.g = 1.0 marker_msg.color.b = 0.0 - marker_msg.color.a = 0.5 # transparency + marker_msg.color.a = 0.5 # transparency marker_msg.pose.position.x = config['x_box_position'] marker_msg.pose.position.y = config['y_box_position'] marker_msg.pose.position.z = config['z_box_position'] @@ -230,15 +249,17 @@ def test_pose_method(self, config): def reconfigureCB(self, config, level): self.marker_msg = self.publish_perception_fov(config) - # test the publication of a pose defined in dynamic dynamic_reconfigure from within this node to see if the node logic is working + # test the publication of a pose defined in dynamic dynamic_reconfigure from within + # this node to see if the node logic is working if self.test_pose: self.test_pose_method(config) return config def start_object_recognition(self): - srv = Server(objBoundsConfig, self.reconfigureCB) + Server(objBoundsConfig, self.reconfigureCB) rospy.spin() + if __name__ == '__main__': rospy.init_node('object_recognition', anonymous=False) object_recognition = ObjRecognitionMockup(test_pose=False) diff --git a/scripts/pick_obj_test_action_client b/scripts/pick_obj_test_action_client index 8e36ee1..d3c3056 100755 --- a/scripts/pick_obj_test_action_client +++ b/scripts/pick_obj_test_action_client @@ -19,23 +19,29 @@ if __name__ == '__main__': object_to_pick = myargv[1] support_surface_name = myargv[2] else: - rospy.logwarn('you have run this node without arguments, at least the mobipick namespace is expected! ( __ns:=mobipick ) ... exiting') + rospy.logwarn( + 'you have run this node without arguments, at least the mobipick namespace is expected!' + ' ( __ns:=mobipick ) ... exiting' + ) sys.exit(1) - timeout = 50.0 # in seconds, default: 50.0 + timeout = 50.0 # in seconds, default: 50.0 pick_object_server_name = 'pick_object' action_client = actionlib.SimpleActionClient(pick_object_server_name, PickObjectAction) rospy.loginfo(f'waiting for {pick_object_server_name} action server') if action_client.wait_for_server(timeout=rospy.Duration.from_sec(30.0)): rospy.loginfo(f'found {pick_object_server_name} action server') goal = PickObjectGoal() - goal.object_name = object_to_pick # pick "any" multimeter + goal.object_name = object_to_pick # pick "any" multimeter goal.support_surface_name = support_surface_name # goal.object_name = 'multimeter_1' # pick a specific multimeter goal.ignore_object_list = [] if len(myargv) > 3: for i in range(len(myargv) - 3): goal.ignore_object_list.append(myargv[i + 3]) - rospy.loginfo(f'sending -> pick {object_to_pick} from {support_surface_name} <- goal to {pick_object_server_name} action server') + rospy.loginfo( + f'sending -> pick {object_to_pick} from {support_surface_name}' + ' <- goal to {pick_object_server_name} action server' + ) if len(goal.ignore_object_list) > 0: rospy.logwarn(f'the following objects: {goal.ignore_object_list} will not be added to the planning scene') else: @@ -45,7 +51,7 @@ if __name__ == '__main__': if action_client.wait_for_result(rospy.Duration.from_sec(timeout)): result = action_client.get_result() rospy.loginfo(f'{pick_object_server_name} is done with execution, resuĺt was = "{result}"') - if result.success == True: + if result.success: rospy.loginfo(f'Succesfully picked {object_to_pick}') else: rospy.logerr(f'Failed to pick {object_to_pick}') diff --git a/scripts/place_obj_test_action_client b/scripts/place_obj_test_action_client index b721c72..4295821 100755 --- a/scripts/place_obj_test_action_client +++ b/scripts/place_obj_test_action_client @@ -22,8 +22,11 @@ if __name__ == '__main__': elif myargv[2] == 'false' or myargv[2] == 'False': observe_before_place = False else: - rospy.logwarn('you have run this node without arguments, at least the mobiplace namespace is expected! ( __ns:=mobiplace )') - timeout = 50.0 # in seconds, default: 50.0 + rospy.logwarn( + 'you have run this node without arguments, at least the mobiplace namespace is expected!' + ' ( __ns:=mobiplace )' + ) + timeout = 50.0 # in seconds, default: 50.0 place_object_server_name = 'place_object' action_client = actionlib.SimpleActionClient(place_object_server_name, PlaceObjectAction) rospy.loginfo(f'waiting for {place_object_server_name} action server') @@ -38,11 +41,11 @@ if __name__ == '__main__': if action_client.wait_for_result(rospy.Duration.from_sec(timeout)): result = action_client.get_result() rospy.loginfo(f'{place_object_server_name} is done with execution, resuĺt was = "{result}"') - if result.success == True: - rospy.loginfo(f'Succesfully placed object') + if result.success: + rospy.loginfo('Succesfully placed object') else: - rospy.logerr(f'Failed to place object') + rospy.logerr('Failed to place object') else: - rospy.logerr(f'Failed to place object, timeout?') + rospy.logerr('Failed to place object, timeout?') else: rospy.logerr(f'action server {place_object_server_name} not available') diff --git a/scripts/publish_tf_world_to_robot b/scripts/publish_tf_world_to_robot index 31bccf3..a74710b 100755 --- a/scripts/publish_tf_world_to_robot +++ b/scripts/publish_tf_world_to_robot @@ -14,6 +14,7 @@ import numpy as np from gazebo_msgs.msg import ModelStates from tf.transformations import quaternion_matrix, quaternion_from_matrix + class PublishTfWorldToRobot: def __init__(self): # subscribe to gazebo model states (gives pose of all existing objects in the simulation @@ -36,7 +37,14 @@ class PublishTfWorldToRobot: for i, obj_pose in enumerate(model_states_msg.pose): if model_states_msg.name[i] == 'mobipick': # Return homogeneous rotation matrix from quaternion - world_2_robot = quaternion_matrix((obj_pose.orientation.x, obj_pose.orientation.y, obj_pose.orientation.z, obj_pose.orientation.w)) + world_2_robot = quaternion_matrix( + ( + obj_pose.orientation.x, + obj_pose.orientation.y, + obj_pose.orientation.z, + obj_pose.orientation.w, + ) + ) world_2_robot[0][3] = obj_pose.position.x world_2_robot[1][3] = obj_pose.position.y world_2_robot[2][3] = obj_pose.position.z @@ -44,10 +52,16 @@ class PublishTfWorldToRobot: # Return quaternion from rotation matrix q = quaternion_from_matrix(robot_2_world) # translation, rotation, time, child, parent - self.tf_broadcaster.sendTransform((robot_2_world[0][3], robot_2_world[1][3], robot_2_world[2][3]), q,\ - rospy.Time.now(), 'world', 'mobipick/base_link') # publish tf with mobipick/base_link as parent not to disturb existing tf tree + self.tf_broadcaster.sendTransform( + (robot_2_world[0][3], robot_2_world[1][3], robot_2_world[2][3]), + q, + rospy.Time.now(), + 'world', + 'mobipick/base_link', + ) # publish tf with mobipick/base_link as parent not to disturb existing tf tree self.loop_rate.sleep() + if __name__ == '__main__': rospy.init_node('publish_tf_world_to_robot', anonymous=False) publish_tf_world_to_robot = PublishTfWorldToRobot() diff --git a/scripts/set_rviz_logger_level.py b/scripts/set_rviz_logger_level.py index dd28485..2953fda 100755 --- a/scripts/set_rviz_logger_level.py +++ b/scripts/set_rviz_logger_level.py @@ -7,15 +7,17 @@ import rospy from roscpp.srv import SetLoggerLevel + def main(): rospy.init_node('set_rviz_logger_level') rospy.wait_for_service('/rviz/set_logger_level') try: set_level = rospy.ServiceProxy('/rviz/set_logger_level', SetLoggerLevel) - resp = set_level('rviz', 'ERROR') + set_level('rviz', 'ERROR') rospy.logwarn("rviz logger level set to ERROR (to supress warnings)") except rospy.ServiceException as e: rospy.logerr(f"Service call failed: {e}") + if __name__ == '__main__': main() diff --git a/scripts/visualize_planning_scene_node b/scripts/visualize_planning_scene_node index 9368462..f709976 100755 --- a/scripts/visualize_planning_scene_node +++ b/scripts/visualize_planning_scene_node @@ -12,6 +12,7 @@ import rospkg from std_msgs.msg import String from grasplan.rqt_planning_scene.visualize_planning_scene import PlanningSceneViz, PlanningSceneVizSettings + class visualizePlanningSceneNode: def __init__(self): package_path = rospkg.RosPack().get_path('grasplan') @@ -69,6 +70,7 @@ class visualizePlanningSceneNode: self.psv.publish_tf() rate.sleep() + if __name__ == '__main__': rospy.init_node('planning_scene_visualization_node', anonymous=False) viz = visualizePlanningSceneNode() diff --git a/setup.py b/setup.py old mode 100644 new mode 100755 index 5c3ff4e..77766d0 --- a/setup.py +++ b/setup.py @@ -4,9 +4,6 @@ from catkin_pkg.python_setup import generate_distutils_setup # for your packages to be recognized by python -d = generate_distutils_setup( - packages=['grasplan'], - package_dir={'grasplan': 'src/grasplan'} -) +d = generate_distutils_setup(packages=['grasplan'], package_dir={'grasplan': 'src/grasplan'}) setup(**d) diff --git a/src/grasplan/grasp_planner/handcoded_grasp_planner.py b/src/grasplan/grasp_planner/handcoded_grasp_planner.py index 01d32bb..9dac305 100755 --- a/src/grasplan/grasp_planner/handcoded_grasp_planner.py +++ b/src/grasplan/grasp_planner/handcoded_grasp_planner.py @@ -11,6 +11,7 @@ from grasplan.tools.common import separate_object_class_from_id + class HandcodedGraspPlanner(GraspPlanningCore): ''' Implement concrete methods out of GraspPlanningCore class @@ -19,6 +20,7 @@ class HandcodedGraspPlanner(GraspPlanningCore): 2) query from paramters fixed transforms wrt object 3) sample around it in roll, pitch, yaw angles as needed ''' + def __init__(self, call_parent_constructor=True): if call_parent_constructor: super().__init__() @@ -26,7 +28,7 @@ def __init__(self, call_parent_constructor=True): # get transforms as a dictionary self.grasp_poses = rospy.get_param('~handcoded_grasp_planner_transforms', []) - rospy.sleep(0.5) # give some time for publisher to register + rospy.sleep(0.5) # give some time for publisher to register rospy.loginfo('handcoded grasp planner object was created') def gen_end_effector_grasp_poses(self, object_name, object_pose, grasp_type): @@ -41,9 +43,9 @@ def gen_end_effector_grasp_poses(self, object_name, object_pose, grasp_type): rot.append(object_pose.pose.orientation.w) euler_rot = tf.transformations.euler_from_quaternion(rot) tf_object_to_world = tf.transformations.euler_matrix(euler_rot[0], euler_rot[1], euler_rot[2]) - tf_object_to_world[0][3] = object_pose.pose.position.x # x - tf_object_to_world[1][3] = object_pose.pose.position.y # y - tf_object_to_world[2][3] = object_pose.pose.position.z # z + tf_object_to_world[0][3] = object_pose.pose.position.x # x + tf_object_to_world[1][3] = object_pose.pose.position.y # y + tf_object_to_world[2][3] = object_pose.pose.position.z # z tf_pose = Pose() pose_array_msg = PoseArray() @@ -54,8 +56,11 @@ def gen_end_effector_grasp_poses(self, object_name, object_pose, grasp_type): object_class = separate_object_class_from_id(object_name)[0] # transform all poses from object reference frame to world reference frame - if not object_class in self.grasp_poses: - rospy.logerr(f'object "{object_class}" not found in dictionary, have you included in handcoded_grasp_planner_transforms parameter?') + if object_class not in self.grasp_poses: + rospy.logerr( + f'object "{object_class}" not found in dictionary, have you included in' + ' handcoded_grasp_planner_transforms parameter?' + ) return pose_array_msg for transform in self.grasp_poses[object_class]['grasp_poses']: @@ -63,9 +68,9 @@ def gen_end_effector_grasp_poses(self, object_name, object_pose, grasp_type): rot = transform['rotation'] euler_rot = tf.transformations.euler_from_quaternion(rot) tf_gripper_to_object = tf.transformations.euler_matrix(euler_rot[0], euler_rot[1], euler_rot[2]) - tf_gripper_to_object[0][3] = transform['translation'][0] # x - tf_gripper_to_object[1][3] = transform['translation'][1] # y - tf_gripper_to_object[2][3] = transform['translation'][2] # z + tf_gripper_to_object[0][3] = transform['translation'][0] # x + tf_gripper_to_object[1][3] = transform['translation'][1] # y + tf_gripper_to_object[2][3] = transform['translation'][2] # z # convert rotation matrix to position and quaternion orientation gripper_pose_wrt_world = np.dot(tf_object_to_world, tf_gripper_to_object) diff --git a/src/grasplan/grasp_planner/link_tf_gt_publisher.py b/src/grasplan/grasp_planner/link_tf_gt_publisher.py index 4022cbe..e218f20 100755 --- a/src/grasplan/grasp_planner/link_tf_gt_publisher.py +++ b/src/grasplan/grasp_planner/link_tf_gt_publisher.py @@ -5,17 +5,18 @@ from gazebo_msgs.msg import LinkStates -''' -subscribe to /gazebo/link_states -take from parameter server a desired link to track -publish link pose as tf -''' class LinkTFgtPublisher: + ''' + subscribe to /gazebo/link_states + take from parameter server a desired link to track + publish link pose as tf + ''' + def __init__(self): # parameters - self.prefix = rospy.get_param('~prefix', 'gazebo_ros_vel/mia_hand::') # robot_name:: - self.link_name = rospy.get_param('~link_name', 'wrist') # the name of the link you want to publish tf + self.prefix = rospy.get_param('~prefix', 'gazebo_ros_vel/mia_hand::') # robot_name:: + self.link_name = rospy.get_param('~link_name', 'wrist') # the name of the link you want to publish tf # subscribe to gazebo model states (gives poses of all existing objects in the simulation rospy.Subscriber('/gazebo/link_states', LinkStates, self.LinkStatesCB) self.rate = rospy.Rate(30) @@ -41,9 +42,13 @@ def publishTF(self): # get link pose link_pose = self.link_state_msg.pose[index] # broadcast object tf - self.tf_broadcaster.sendTransform((link_pose.position.x, link_pose.position.y, link_pose.position.z),\ - (link_pose.orientation.x, link_pose.orientation.y, link_pose.orientation.z, link_pose.orientation.w),\ - rospy.Time.now(), self.link_name, 'world') + self.tf_broadcaster.sendTransform( + (link_pose.position.x, link_pose.position.y, link_pose.position.z), + (link_pose.orientation.x, link_pose.orientation.y, link_pose.orientation.z, link_pose.orientation.w), + rospy.Time.now(), + self.link_name, + 'world', + ) def start_link_tf_ft_pub(self): while not rospy.is_shutdown(): @@ -53,6 +58,7 @@ def start_link_tf_ft_pub(self): self.publishTF() self.rate.sleep() + if __name__ == '__main__': rospy.init_node('link_tf_gt_pub', anonymous=False) link_tf_gt_pub = LinkTFgtPublisher() diff --git a/src/grasplan/grasp_planner/simple_pregrasp_planner.py b/src/grasplan/grasp_planner/simple_pregrasp_planner.py index 8672bfd..dd2c5f1 100755 --- a/src/grasplan/grasp_planner/simple_pregrasp_planner.py +++ b/src/grasplan/grasp_planner/simple_pregrasp_planner.py @@ -5,6 +5,7 @@ from grasplan.pose_generator import PoseGenerator from geometry_msgs.msg import PoseStamped + class SimpleGraspPlanner(GraspPlanningCore): ''' Implement concrete methods out of GraspPlanningCore class @@ -13,6 +14,7 @@ class SimpleGraspPlanner(GraspPlanningCore): 2) replace it with a prerecorded "example" orientation 3) sample around it in roll, pitch, yaw angles ''' + def __init__(self): super().__init__() self.pose_generator = PoseGenerator() @@ -25,12 +27,14 @@ def __init__(self): # setup publishers for visualization purposes self.grasp_pose_pub = rospy.Publisher('~visualization/grasp_pose', PoseStamped, queue_size=1) - rospy.sleep(0.5) # give some time for publisher to register + rospy.sleep(0.5) # give some time for publisher to register rospy.loginfo('simple pregrasp planner object was created') def generate_grasp_pose(self, object_pose, grasp_type): grasp_pose = PoseStamped() - grasp_pose.header.frame_id = object_pose.header.frame_id # object pose must be expressed w.r.t : self.robot.get_planning_frame() + grasp_pose.header.frame_id = ( + object_pose.header.frame_id + ) # object pose must be expressed w.r.t : self.robot.get_planning_frame() # take position from perceived object translation = [object_pose.pose.position.x, object_pose.pose.position.y, object_pose.pose.position.z] diff --git a/src/grasplan/grasp_planner/teleop_twist_keyboard.py b/src/grasplan/grasp_planner/teleop_twist_keyboard.py index 7492c60..df321b2 100755 --- a/src/grasplan/grasp_planner/teleop_twist_keyboard.py +++ b/src/grasplan/grasp_planner/teleop_twist_keyboard.py @@ -1,15 +1,12 @@ #!/usr/bin/env python3 -from __future__ import print_function - import threading - -import roslib; roslib.load_manifest('teleop_twist_keyboard') import rospy - from geometry_msgs.msg import Twist - -import sys, select, termios, tty +import sys +import select +import termios +import tty msg = """ Reading from the keyboard and Publishing to Twist! @@ -32,33 +29,34 @@ """ moveBindings = { - '8':(1,0,0,0,0,0), # +x - '2':(-1,0,0,0,0,0), # -x - '6':(0,1,0,0,0,0), # +y - '4':(0,-1,0,0,0,0), # -y - '9':(0,0,1,0,0,0), # +z - '7':(0,0,-1,0,0,0), # -z - 'r':(0,0,0,1,0,0), # +roll - 'R':(0,0,0,-1,0,0), # -roll - 'p':(0,0,0,0,1,0), # +pitch - 'P':(0,0,0,0,-1,0), # -pitch - 'y':(0,0,0,0,0,1), # +yaw - 'Y':(0,0,0,0,0,-1), # -yaw - } - -speedBindings={ - 'q':(1.1,1.1), - 'z':(.9,.9), - 'w':(1.1,1), - 'x':(.9,1), - 'e':(1,1.1), - 'c':(1,.9), - } + '8': (1, 0, 0, 0, 0, 0), # +x + '2': (-1, 0, 0, 0, 0, 0), # -x + '6': (0, 1, 0, 0, 0, 0), # +y + '4': (0, -1, 0, 0, 0, 0), # -y + '9': (0, 0, 1, 0, 0, 0), # +z + '7': (0, 0, -1, 0, 0, 0), # -z + 'r': (0, 0, 0, 1, 0, 0), # +roll + 'R': (0, 0, 0, -1, 0, 0), # -roll + 'p': (0, 0, 0, 0, 1, 0), # +pitch + 'P': (0, 0, 0, 0, -1, 0), # -pitch + 'y': (0, 0, 0, 0, 0, 1), # +yaw + 'Y': (0, 0, 0, 0, 0, -1), # -yaw +} + +speedBindings = { + 'q': (1.1, 1.1), + 'z': (0.9, 0.9), + 'w': (1.1, 1), + 'x': (0.9, 1), + 'e': (1, 1.1), + 'c': (1, 0.9), +} + class PublishThread(threading.Thread): def __init__(self, rate): super(PublishThread, self).__init__() - self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size = 1) + self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size=1) self.x = 0.0 self.y = 0.0 self.z = 0.0 @@ -151,9 +149,10 @@ def getKey(key_timeout): def vels(speed, turn): - return "currently:\tspeed %s\tturn %s " % (speed,turn) + return "currently:\tspeed %s\tturn %s " % (speed, turn) + -if __name__=="__main__": +if __name__ == "__main__": settings = termios.tcgetattr(sys.stdin) rospy.init_node('teleop_twist_keyboard') @@ -180,8 +179,8 @@ def vels(speed, turn): pub_thread.update(x, y, z, roll, pitch, yaw, speed, turn) print(msg) - print(vels(speed,turn)) - while(1): + print(vels(speed, turn)) + while 1: key = getKey(key_timeout) if key in moveBindings.keys(): x = moveBindings[key][0] @@ -194,8 +193,8 @@ def vels(speed, turn): speed = speed * speedBindings[key][0] turn = turn * speedBindings[key][1] - print(vels(speed,turn)) - if (status == 14): + print(vels(speed, turn)) + if status == 14: print(msg) status = (status + 1) % 15 else: @@ -209,9 +208,9 @@ def vels(speed, turn): roll = 0 pitch = 0 yaw = 0 - if (key == '\x03'): + if key == '\x03': break - + pub_thread.update(x, y, z, roll, pitch, yaw, speed, turn) except Exception as e: diff --git a/src/grasplan/grasp_planner/tf_gripper_listener.py b/src/grasplan/grasp_planner/tf_gripper_listener.py index a9efbc6..f162c9e 100755 --- a/src/grasplan/grasp_planner/tf_gripper_listener.py +++ b/src/grasplan/grasp_planner/tf_gripper_listener.py @@ -1,9 +1,9 @@ #!/usr/bin/env python3 -import time import rospy import tf + # function taken from : https://stackoverflow.com/questions/3136059/getting-one-value-from-a-tuple # reads a single key pressed def read_single_keypress(): @@ -14,40 +14,50 @@ def read_single_keypress(): then read the single keystroke then revert stdin back after reading the keystroke. - Returns a tuple of characters of the key that was pressed - on Linux, - pressing keys like up arrow results in a sequence of characters. Returns + Returns a tuple of characters of the key that was pressed - on Linux, + pressing keys like up arrow results in a sequence of characters. Returns ('\x03',) on KeyboardInterrupt which can happen when a signal gets handled. """ - import termios, fcntl, sys, os + import termios + import fcntl + import sys + import os + fd = sys.stdin.fileno() # save old state flags_save = fcntl.fcntl(fd, fcntl.F_GETFL) attrs_save = termios.tcgetattr(fd) # make raw - the way to do this comes from the termios(3) man page. - attrs = list(attrs_save) # copy the stored version to update + attrs = list(attrs_save) # copy the stored version to update # iflag - attrs[0] &= ~(termios.IGNBRK | termios.BRKINT | termios.PARMRK - | termios.ISTRIP | termios.INLCR | termios. IGNCR - | termios.ICRNL | termios.IXON ) + attrs[0] &= ~( + termios.IGNBRK + | termios.BRKINT + | termios.PARMRK + | termios.ISTRIP + | termios.INLCR + | termios.IGNCR + | termios.ICRNL + | termios.IXON + ) # oflag attrs[1] &= ~termios.OPOST # cflag - attrs[2] &= ~(termios.CSIZE | termios. PARENB) + attrs[2] &= ~(termios.CSIZE | termios.PARENB) attrs[2] |= termios.CS8 # lflag - attrs[3] &= ~(termios.ECHONL | termios.ECHO | termios.ICANON - | termios.ISIG | termios.IEXTEN) + attrs[3] &= ~(termios.ECHONL | termios.ECHO | termios.ICANON | termios.ISIG | termios.IEXTEN) termios.tcsetattr(fd, termios.TCSANOW, attrs) # turn off non-blocking fcntl.fcntl(fd, fcntl.F_SETFL, flags_save & ~os.O_NONBLOCK) # read a single keystroke ret = [] try: - ret.append(sys.stdin.read(1)) # returns a single character + ret.append(sys.stdin.read(1)) # returns a single character fcntl.fcntl(fd, fcntl.F_SETFL, flags_save | os.O_NONBLOCK) - c = sys.stdin.read(1) # returns a single character + c = sys.stdin.read(1) # returns a single character while len(c) > 0: ret.append(c) c = sys.stdin.read(1) @@ -59,6 +69,7 @@ def read_single_keypress(): fcntl.fcntl(fd, fcntl.F_SETFL, flags_save) return tuple(ret) + class TFGripperListener: def __init__(self): self.listener = tf.TransformListener() @@ -70,7 +81,7 @@ def write_to_file(self, list_of_strings): if len(list_of_strings) == 3: rospy.logwarn('grasp poses file will not be generated (user did not pressed enter at least once)') return - f = open(self.file_path,'w+') + f = open(self.file_path, 'w+') for string in list_of_strings: f.write(string + '\n') f.close() @@ -87,7 +98,9 @@ def start(self): if key[0] == 'q' or key[0] == 'Q': break try: - (trans,rot) = self.listener.lookupTransform(self.object_ref_frame, self.end_effector_link, rospy.Time(0)) + (trans, rot) = self.listener.lookupTransform( + self.object_ref_frame, self.end_effector_link, rospy.Time(0) + ) stream_list.append(f'{tab}{tab}-') # translation translation_str = f'{tab}{tab}{tab}translation: [{trans[0]:.6f}, {trans[1]:.6f}, {trans[2]:.6f}]' @@ -100,10 +113,13 @@ def start(self): stream_list.append(rotation_str) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): - rospy.logerr(f'LookupTransform failed between frames: {self.object_ref_frame} and {self.end_effector_link}') + rospy.logerr( + f'LookupTransform failed between frames: {self.object_ref_frame} and {self.end_effector_link}' + ) rospy.loginfo(f'writing to file: {self.file_path}') self.write_to_file(stream_list) + if __name__ == '__main__': rospy.init_node('tf_listener') tf_gripper_listener = TFGripperListener() diff --git a/src/grasplan/grasp_planning_core.py b/src/grasplan/grasp_planning_core.py index ac937f3..d72b6c0 100755 --- a/src/grasplan/grasp_planning_core.py +++ b/src/grasplan/grasp_planning_core.py @@ -9,11 +9,13 @@ from grasplan.tools.common import separate_object_class_from_id + class GraspPlanningCore: ''' Abstract class that defines classes to interact with a grasp planning module It cannot be used by itself, you need to inherit from it and implement virtual methods ''' + def __init__(self): # parameters @@ -82,11 +84,17 @@ def make_grasps_msgs(self, object_name, object_pose, end_effector_frame, grasp_t # The internal posture of the hand for the pre-grasp # only positions are used object_class = separate_object_class_from_id(object_name)[0] - g.pre_grasp_posture = self.make_gripper_trajectory(self.gripper_open_distance, self.distance_gripper_open_per_obj, object_class=object_class) + g.pre_grasp_posture = self.make_gripper_trajectory( + self.gripper_open_distance, self.distance_gripper_open_per_obj, object_class=object_class + ) # The approach direction to take before picking an object - g.pre_grasp_approach = self.make_gripper_translation_msg(end_effector_frame, - min_dist=self.pre_grasp_approach_min_dist, desired=self.pre_grasp_approach_desired, axis=self.pre_grasp_approach_axis) + g.pre_grasp_approach = self.make_gripper_translation_msg( + end_effector_frame, + min_dist=self.pre_grasp_approach_min_dist, + desired=self.pre_grasp_approach_desired, + axis=self.pre_grasp_approach_axis, + ) # The position of the end-effector for the grasp. This is the pose of # the 'parent_link' of the end-effector, not actually the pose of any @@ -96,11 +104,17 @@ def make_grasps_msgs(self, object_name, object_pose, end_effector_frame, grasp_t # The internal posture of the hand for the grasp # positions and efforts are used - g.grasp_posture = self.make_gripper_trajectory(self.gripper_close_distance, self.distance_gripper_close_per_obj, object_class=object_class) + g.grasp_posture = self.make_gripper_trajectory( + self.gripper_close_distance, self.distance_gripper_close_per_obj, object_class=object_class + ) # The retreat direction to take after a grasp has been completed (object is attached) - g.post_grasp_retreat = self.make_gripper_translation_msg(self.post_grasp_retreat_frame_id, - min_dist=self.post_grasp_retreat_min_dist, desired=self.post_grasp_retreat_desired, axis=self.post_grasp_retreat_axis) + g.post_grasp_retreat = self.make_gripper_translation_msg( + self.post_grasp_retreat_frame_id, + min_dist=self.post_grasp_retreat_min_dist, + desired=self.post_grasp_retreat_desired, + axis=self.post_grasp_retreat_axis, + ) # the maximum contact force to use while grasping (<=0 to disable) g.max_contact_force = self.max_contact_force diff --git a/src/grasplan/insert.py b/src/grasplan/insert.py index eff78a4..8fcd6a7 100755 --- a/src/grasplan/insert.py +++ b/src/grasplan/insert.py @@ -11,29 +11,38 @@ from grasplan.tools.support_plane_tools import gen_insert_poses_from_obj, compute_object_height_for_insertion from grasplan.tools.moveit_errors import print_moveit_error from object_pose_msgs.msg import ObjectList -from moveit_msgs.msg import PlaceAction, PlaceGoal # PlaceLocation, GripperTranslation, PlanningOptions, Constraints, OrientationConstraint +from moveit_msgs.msg import PlaceAction from moveit_msgs.msg import MoveItErrorCodes from std_srvs.srv import Empty from pose_selector.srv import ClassQuery -from grasplan.tools.common import objectToPick # name is misleading, in this case we want to insert an object in it +from grasplan.tools.common import objectToPick # name is misleading, in this case we want to insert an object in it from grasplan.msg import InsertObjectAction, InsertObjectResult -class InsertTools(): + +class InsertTools: def __init__(self): # create instance of place - self.place = PlaceTools(action_server_required=False) # we will advertise our own action lib server for insertion + self.place = PlaceTools( + action_server_required=False + ) # we will advertise our own action lib server for insertion # parameters - pick_pose_selector_class_query_srv_name = rospy.get_param('~pick_pose_selector_class_query_srv_name', '/pose_selector_class_query') + pick_pose_selector_class_query_srv_name = rospy.get_param( + '~pick_pose_selector_class_query_srv_name', '/pose_selector_class_query' + ) self.disentangle_required = rospy.get_param('~disentangle_required', False) self.poses_to_go_before_insert = rospy.get_param('~poses_to_go_before_insert', []) rospy.loginfo(f'waiting for pose selector services: {pick_pose_selector_class_query_srv_name}') rospy.wait_for_service(pick_pose_selector_class_query_srv_name, 30.0) - self.pick_pose_selector_class_query_srv = rospy.ServiceProxy(pick_pose_selector_class_query_srv_name, ClassQuery) + self.pick_pose_selector_class_query_srv = rospy.ServiceProxy( + pick_pose_selector_class_query_srv_name, ClassQuery + ) self.insert_poses_pub = rospy.Publisher('place_object_node/place_poses', ObjectList, queue_size=50) - self.insert_action_server = actionlib.SimpleActionServer('insert_object', InsertObjectAction, self.insert_obj_action_callback, False) + self.insert_action_server = actionlib.SimpleActionServer( + 'insert_object', InsertObjectAction, self.insert_obj_action_callback, False + ) self.insert_action_server.start() # give some time for publishers and Subscribers to register @@ -41,9 +50,9 @@ def __init__(self): def insert_obj_action_callback(self, goal): success = False - for i in range(2): # 0, 1 = 2 attemps + for i in range(2): # 0, 1 = 2 attemps rospy.loginfo(f'Insert object: attempt number {i + 1}') - if i == 0: # first try, optimistic, keep same orientation as support object + if i == 0: # first try, optimistic, keep same orientation as support object # disentangle cable only on first attempt override_disentangle_dont_doit = False override_observe_before_place_dont_doit = False @@ -51,17 +60,20 @@ def insert_obj_action_callback(self, goal): # do not disentangle if we dont go to observe arm pose if not goal.observe_before_insert: override_disentangle_dont_doit = True - else: # second try, generate 360 degree orientations + else: # second try, generate 360 degree orientations override_disentangle_dont_doit = True override_observe_before_place_dont_doit = True same_orientation_as_support_obj = False # do not disentangle if we dont go to observe arm pose if not goal.observe_before_insert: override_disentangle_dont_doit = True - if self.insert_object(goal.support_surface_name, observe_before_insert=goal.observe_before_insert,\ - same_orientation_as_support_obj=same_orientation_as_support_obj,\ - override_disentangle_dont_doit=override_disentangle_dont_doit,\ - override_observe_before_place_dont_doit=override_observe_before_place_dont_doit): + if self.insert_object( + goal.support_surface_name, + observe_before_insert=goal.observe_before_insert, + same_orientation_as_support_obj=same_orientation_as_support_obj, + override_disentangle_dont_doit=override_disentangle_dont_doit, + override_observe_before_place_dont_doit=override_observe_before_place_dont_doit, + ): success = True break if success: @@ -77,18 +89,32 @@ def get_support_object_pose(self, support_object): # query pose selector resp = self.pick_pose_selector_class_query_srv(support_object.obj_class) if len(resp.poses) == 0: - rospy.logerr(f'Object of class {support_object.obj_class} was not perceived, therefore its pose is not available and cannot be picked') + rospy.logerr( + f'Object of class {support_object.obj_class} was not perceived, therefore its pose is not available' + ' and cannot be picked' + ) return None # at least one object of the same class as the object we want to pick was perceived, continue for pose in resp.poses: if pose.instance_id == support_object.id: - rospy.loginfo(f'success at getting support object pose: {support_object.get_object_class_and_id_as_string()}') - return pose # of type object_pose_msgs/ObjectPose.msg - rospy.logerr(f'At least one object of the class {support_object.obj_class} was perceived but is not the one you want, with id: {support_object.id}') + rospy.loginfo( + f'success at getting support object pose: {support_object.get_object_class_and_id_as_string()}' + ) + return pose # of type object_pose_msgs/ObjectPose.msg + rospy.logerr( + f'At least one object of the class {support_object.obj_class} was perceived but is not the one you want,' + ' with id: {support_object.id}' + ) return None - def insert_object(self, support_object_name_as_string, observe_before_insert=False, same_orientation_as_support_obj=False,\ - override_disentangle_dont_doit=False, override_observe_before_place_dont_doit=False): + def insert_object( + self, + support_object_name_as_string, + observe_before_insert=False, + same_orientation_as_support_obj=False, + override_disentangle_dont_doit=False, + override_observe_before_place_dont_doit=False, + ): ''' use place functionality by creating 1 pose above the support_object_name_as_string object for now NOTE: support_object_name_as_string has an id @@ -106,8 +132,10 @@ def insert_object(self, support_object_name_as_string, observe_before_insert=Fal object_to_be_inserted = list(self.place.scene.get_attached_objects().keys())[0] object_class_tbi = separate_object_class_from_id(object_to_be_inserted)[0] - rospy.loginfo(f'received request to insert the object I am currently holding ({object_to_be_inserted})\ - into {support_object.get_object_class_and_id_as_string()}') + rospy.loginfo( + f'received request to insert the object I am currently holding ({object_to_be_inserted})\ + into {support_object.get_object_class_and_id_as_string()}' + ) # clear pose selector before starting to place in case some data is left over from previous runs self.place.place_pose_selector_clear_srv() @@ -117,9 +145,9 @@ def insert_object(self, support_object_name_as_string, observe_before_insert=Fal # optionally find the support object: look at table, update planning scene self.place.move_arm_to_posture(self.place.arm_pose_with_objs_in_fov) # activate pick pose selector to observe table - resp = self.place.activate_pick_pose_selector_srv(True) - rospy.sleep(0.5) # give some time to observe - resp = self.place.activate_pick_pose_selector_srv(False) + self.place.activate_pick_pose_selector_srv(True) + rospy.sleep(0.5) # give some time to observe + self.place.activate_pick_pose_selector_srv(False) self.place.add_objs_to_planning_scene() # allow disentangle to happen only on first place attempt, no need to do it every time @@ -138,9 +166,13 @@ def insert_object(self, support_object_name_as_string, observe_before_insert=Fal if support_object_pose is None: return False - place_poses_as_object_list_msg = gen_insert_poses_from_obj(object_class_tbi, support_object_pose,\ - compute_object_height_for_insertion(object_class_tbi, support_object.obj_class),\ - frame_id=self.place.global_reference_frame, same_orientation_as_support_obj=same_orientation_as_support_obj) + place_poses_as_object_list_msg = gen_insert_poses_from_obj( + object_class_tbi, + support_object_pose, + compute_object_height_for_insertion(object_class_tbi, support_object.obj_class), + frame_id=self.place.global_reference_frame, + same_orientation_as_support_obj=same_orientation_as_support_obj, + ) # send places poses to place pose selector for visualization purposes self.insert_poses_pub.publish(place_poses_as_object_list_msg) @@ -150,9 +182,16 @@ def insert_object(self, support_object_name_as_string, observe_before_insert=Fal if action_client.wait_for_server(timeout=rospy.Duration.from_sec(2.0)): rospy.loginfo(f'found {self.place.place_object_server_name} action server') - goal = self.place.make_place_goal_msg(object_to_be_inserted, support_object.get_object_class_and_id_as_string(), place_poses_as_object_list_msg, use_path_constraints=False) - - rospy.loginfo(f'sending place {object_to_be_inserted} goal to {self.place.place_object_server_name} action server') + goal = self.place.make_place_goal_msg( + object_to_be_inserted, + support_object.get_object_class_and_id_as_string(), + place_poses_as_object_list_msg, + use_path_constraints=False, + ) + + rospy.loginfo( + f'sending place {object_to_be_inserted} goal to {self.place.place_object_server_name} action server' + ) action_client.send_goal(goal) rospy.loginfo(f'waiting for result from {self.place.place_object_server_name} action server') if action_client.wait_for_result(rospy.Duration.from_sec(self.place.timeout)): @@ -160,13 +199,13 @@ def insert_object(self, support_object_name_as_string, observe_before_insert=Fal # handle moveit pick result if result.error_code.val == MoveItErrorCodes.SUCCESS: - rospy.loginfo(f'Successfully inserted object') + rospy.loginfo('Successfully inserted object') self.place.place_pose_selector_clear_srv() # clear possible place poses markers in rviz self.place.clear_place_poses_markers() return True else: - rospy.logerr(f'insert object failed') + rospy.logerr('insert object failed') self.place.place_pose_selector_clear_srv() # clear possible place poses markers in rviz # self.place.clear_place_poses_markers() # leave markers for debugging if failed to insert @@ -181,7 +220,8 @@ def start_insert_node(self): rospy.loginfo('ready to insert objects') rospy.spin() -if __name__=='__main__': + +if __name__ == '__main__': rospy.init_node('insert_object_node', anonymous=False) insert = InsertTools() insert.start_insert_node() diff --git a/src/grasplan/pick.py b/src/grasplan/pick.py index f099678..934af8a 100755 --- a/src/grasplan/pick.py +++ b/src/grasplan/pick.py @@ -11,7 +11,6 @@ import rospy import actionlib -import tf import moveit_commander from tf import TransformListener @@ -25,7 +24,8 @@ from grasplan.tools.common import objectToPick from visualization_msgs.msg import Marker, MarkerArray -class PickTools(): + +class PickTools: def __init__(self): # parameters @@ -42,7 +42,8 @@ def __init__(self): self.clear_octomap_flag = rospy.get_param('~clear_octomap', False) self.poses_to_go_before_pick = rospy.get_param('~poses_to_go_before_pick', []) self.list_of_disentangle_objects = rospy.get_param('~list_of_disentangle_objects', []) - # if true the arm is moved to a pose where objects are inside fov and pose selector is triggered to accept obj pose updates + # if true the arm is moved to a pose where objects are inside fov and pose selector is triggered + # to accept obj pose updates self.perceive_object = rospy.get_param('~perceive_object', True) # the arm pose where the objects are inside the fov (used to move the arm to perceive objs right after) self.arm_pose_with_objs_in_fov = rospy.get_param('~arm_pose_with_objs_in_fov', 'observe100cm_right') @@ -59,11 +60,18 @@ def __init__(self): # service clients pose_selector_activate_srv_name = rospy.get_param('~pose_selector_activate_srv_name', '/pose_selector_activate') - pose_selector_class_query_srv_name = rospy.get_param('~pose_selector_class_query_srv_name', '/pose_selector_class_query') - pose_selector_get_all_poses_srv_name = rospy.get_param('~pose_selector_get_all_poses_srv_name', '/pose_selector_get_all') + pose_selector_class_query_srv_name = rospy.get_param( + '~pose_selector_class_query_srv_name', '/pose_selector_class_query' + ) + pose_selector_get_all_poses_srv_name = rospy.get_param( + '~pose_selector_get_all_poses_srv_name', '/pose_selector_get_all' + ) pose_selector_delete_srv_name = rospy.get_param('~pose_selector_delete_srv_name', '/pose_selector_delete') - rospy.loginfo(f'waiting for pose selector services: {pose_selector_activate_srv_name}, {pose_selector_class_query_srv_name},\ - {pose_selector_get_all_poses_srv_name}, {pose_selector_delete_srv_name}') + rospy.loginfo( + f'waiting for pose selector services: {pose_selector_activate_srv_name},' + ' {pose_selector_class_query_srv_name}, {pose_selector_get_all_poses_srv_name},' + ' {pose_selector_delete_srv_name}' + ) # if wait_for_service fails, it will throw a # rospy.exceptions.ROSException, and the node will exit (as long as # this happens before moveit_commander.roscpp_initialize()). @@ -90,7 +98,9 @@ def __init__(self): # moveit_commander.roscpp_initialize overwrites the signal handler, # so if a RuntimeError occurs here, we have to manually call # signal_shutdown() in order for the node to properly exit. - rospy.logfatal('grasplan pick server could not connect to Moveit in time, exiting! \n' + traceback.format_exc()) + rospy.logfatal( + 'grasplan pick server could not connect to Moveit in time, exiting! \n' + traceback.format_exc() + ) rospy.signal_shutdown('fatal error') # to publish object pose for debugging purposes @@ -100,14 +110,18 @@ def __init__(self): self.event_out_pub = rospy.Publisher('~event_out', String, queue_size=1) self.trigger_perception_pub = rospy.Publisher('/object_recognition/event_in', String, queue_size=1) self.pick_grasps_marker_array_pub = rospy.Publisher('/gripper', MarkerArray, queue_size=1) - self.pose_selector_objects_marker_array_pub = rospy.Publisher('/pose_selector_objects', MarkerArray, queue_size=1) + self.pose_selector_objects_marker_array_pub = rospy.Publisher( + '/pose_selector_objects', MarkerArray, queue_size=1 + ) # subscribers - self.grasp_type = 'side_grasp' # only used for simple_pregrasp_planner at the moment + self.grasp_type = 'side_grasp' # only used for simple_pregrasp_planner at the moment rospy.Subscriber('~grasp_type', String, self.graspTypeCB) # offer action lib server - self.pick_action_server = actionlib.SimpleActionServer('pick_object', PickObjectAction, self.pick_obj_action_callback, False) + self.pick_action_server = actionlib.SimpleActionServer( + 'pick_object', PickObjectAction, self.pick_obj_action_callback, False + ) self.pick_action_server.start() rospy.loginfo('pick node ready!') @@ -129,14 +143,17 @@ def transform_pose(self, pose, target_reference_frame): def make_object_pose_and_add_objs_to_planning_scene(self, object_to_pick, ignore_object_list=[]): ''' - ignore_object_list: if an object is inside another one, you can add it to the ignore_object_list and it will not be - added to the planning scene, but it will rather be removed from the planning scene + ignore_object_list: if an object is inside another one, you can add it to the ignore_object_list and it will + not be added to the planning scene, but it will rather be removed from the planning scene ''' assert isinstance(object_to_pick, objectToPick) # query pose selector resp = self.pose_selector_class_query_srv(object_to_pick.obj_class) if len(resp.poses) == 0: - rospy.logerr(f'Object of class {object_to_pick.obj_class} was not perceived, therefore its pose is not available and cannot be picked') + rospy.logerr( + f'Object of class {object_to_pick.obj_class} was not perceived, therefore its pose is not available and' + ' cannot be picked' + ) return None, None, None # at least one object of the same class as the object we want to pick was perceived, continue object_to_pick_id = object_to_pick.id @@ -165,7 +182,9 @@ def make_object_pose_and_add_objs_to_planning_scene(self, object_to_pick, ignore object_to_pick_bounding_box = copy.deepcopy(object_bounding_box) object_to_pick_id = copy.deepcopy(pose_selector_object.instance_id) object_found = True - rospy.loginfo(f'found an instance of the object class you want to pick in pose selector: {object_name}') + rospy.loginfo( + f'found an instance of the object class you want to pick in pose selector: {object_name}' + ) elif object_to_pick.get_object_class_and_id_as_string() == object_name: object_to_pick_pose = copy.deepcopy(pose_stamped_msg) object_to_pick_bounding_box = copy.deepcopy(object_bounding_box) @@ -182,9 +201,16 @@ def make_object_pose_and_add_objs_to_planning_scene(self, object_to_pick, ignore # add all perceived objects to planning scene (one at at time) self.scene.add_box(object_name, pose_stamped_msg, object_bounding_box) if not object_found: - rospy.logerr(f'the specific object you want to pick was not found : {object_to_pick.get_object_class_and_id_as_string()}') + rospy.logerr( + 'the specific object you want to pick was not found:' + ' {object_to_pick.get_object_class_and_id_as_string()}' + ) return None, None, None - return self.transform_pose(object_to_pick_pose, self.robot.get_planning_frame()), object_to_pick_bounding_box, object_to_pick_id + return ( + self.transform_pose(object_to_pick_pose, self.robot.get_planning_frame()), + object_to_pick_bounding_box, + object_to_pick_id, + ) def clean_scene(self): ''' @@ -262,27 +288,24 @@ def pick_object(self, object_name_as_string, support_surface_name, grasp_type, i object_to_pick = objectToPick(object_name_as_string) # open gripper - #rospy.loginfo('gripper will open now') - #self.move_gripper_to_posture('open') + # rospy.loginfo('gripper will open now') + # self.move_gripper_to_posture('open') # detach (all) object if any from the gripper if self.detach_all_objects_flag: self.detach_all_objects() - # flag to keep track of the state of the grasp - success = False - # ::::::::: perceive object to be picked (optional, read from parameter server if this is required) if self.perceive_object: # send arm to a pose where objects are inside fov self.move_arm_to_posture(self.arm_pose_with_objs_in_fov) # populate pose selector with pose information - resp = self.activate_pose_selector_srv(True) + self.activate_pose_selector_srv(True) # wait until pose selector gets updates rospy.sleep(4.0) # deactivate pose selector detections - resp = self.activate_pose_selector_srv(False) + self.activate_pose_selector_srv(False) # ::::::::: setup planning scene rospy.loginfo('setup planning scene') @@ -309,8 +332,11 @@ def pick_object(self, object_name_as_string, support_surface_name, grasp_type, i table_pose.pose.orientation.w = planning_scene_box['box_orientation_w'] self.scene.add_box(planning_scene_box['scene_name'], table_pose, (box_x, box_y, box_z)) - # add all perceived objects of interest to planning scene and return the pose, bb, and id of the object to be picked - object_pose, bounding_box, id = self.make_object_pose_and_add_objs_to_planning_scene(object_to_pick, ignore_object_list=ignore_object_list) + # add all perceived objects of interest to planning scene and return the pose, bb, and id of the + # object to be picked + object_pose, bounding_box, id = self.make_object_pose_and_add_objs_to_planning_scene( + object_to_pick, ignore_object_list=ignore_object_list + ) if object_pose is None: return False @@ -319,7 +345,7 @@ def pick_object(self, object_name_as_string, support_surface_name, grasp_type, i if id is not None: object_to_pick.set_id(id) - self.obj_pose_pub.publish(object_pose) # publish object pose for visualization purposes + self.obj_pose_pub.publish(object_pose) # publish object pose for visualization purposes # print objects that were added to the planning scene rospy.loginfo(f'planning scene objects: {self.scene.get_known_object_names()}') @@ -329,12 +355,16 @@ def pick_object(self, object_name_as_string, support_surface_name, grasp_type, i self.move_arm_to_posture(self.pregrasp_posture) # ::::::::: pick - rospy.loginfo(f'picking object now') + rospy.loginfo('picking object now') # generate a list of moveit grasp messages, poses are also published for visualization purposes - grasps = self.grasp_planner.make_grasps_msgs(object_to_pick.get_object_class_and_id_as_string(),\ - object_pose, self.robot.arm.get_end_effector_link(), grasp_type) - + grasps = self.grasp_planner.make_grasps_msgs( + object_to_pick.get_object_class_and_id_as_string(), + object_pose, + self.robot.arm.get_end_effector_link(), + grasp_type, + ) + # clear octomap from the planning scene if needed if self.clear_octomap_flag: self.clear_octomap() @@ -346,7 +376,7 @@ def pick_object(self, object_name_as_string, support_surface_name, grasp_type, i self.move_arm_to_posture(arm_pose) # try to pick object with moveit - #result = self._pick_with_moveit_commander(object_to_pick, grasps, support_surface_name) + # result = self._pick_with_moveit_commander(object_to_pick, grasps, support_surface_name) result = self._pick_with_action(object_to_pick, grasps, support_surface_name) # handle moveit pick result if result == MoveItErrorCodes.SUCCESS: @@ -358,7 +388,7 @@ def pick_object(self, object_name_as_string, support_surface_name, grasp_type, i self.clear_mesh_markers(namespace='object', publisher=self.pose_selector_objects_marker_array_pub) return True else: - rospy.logerr(f'grasp failed') + rospy.logerr('grasp failed') print_moveit_error(result) return False @@ -388,8 +418,10 @@ def _pick_with_action(self, object_to_pick, grasps, support_surface_name): goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.replan_delay = 2.0 - rospy.loginfo(f'sending pick {object_to_pick.get_object_class_and_id_as_string()} goal ' - f'to {rospy.resolve_name(PICK_OBJECT_SERVER_NAME)} action server') + rospy.loginfo( + f'sending pick {object_to_pick.get_object_class_and_id_as_string()} goal ' + f'to {rospy.resolve_name(PICK_OBJECT_SERVER_NAME)} action server' + ) action_client.send_goal(goal) rospy.loginfo(f'waiting for result from {rospy.resolve_name(PICK_OBJECT_SERVER_NAME)} action server') if action_client.wait_for_result(rospy.Duration.from_sec(60.0)): @@ -408,7 +440,8 @@ def start_pick_node(self): # shutdown moveit cpp interface before exit moveit_commander.roscpp_shutdown() -if __name__=='__main__': + +if __name__ == '__main__': rospy.init_node('pick_object_node', anonymous=False) pick = PickTools() pick.start_pick_node() diff --git a/src/grasplan/place.py b/src/grasplan/place.py index 0a5eb10..309724f 100755 --- a/src/grasplan/place.py +++ b/src/grasplan/place.py @@ -12,35 +12,49 @@ import moveit_commander import traceback -from grasplan.tools.support_plane_tools import obj_to_plane, adjust_plane, gen_place_poses_from_plane, make_plane_marker_msg +from grasplan.tools.support_plane_tools import ( + obj_to_plane, + adjust_plane, + gen_place_poses_from_plane, + make_plane_marker_msg, +) from grasplan.tools.common import separate_object_class_from_id from grasplan.tools.moveit_errors import print_moveit_error from std_srvs.srv import Empty, SetBool, Trigger from object_pose_msgs.msg import ObjectList import tf2_geometry_msgs from geometry_msgs.msg import Vector3Stamped, PoseStamped, TransformStamped, PointStamped, Point, Transform -from moveit_msgs.msg import PlaceAction, PlaceGoal, PlaceLocation, GripperTranslation, PlanningOptions, Constraints, OrientationConstraint +from moveit_msgs.msg import ( + PlaceAction, + PlaceGoal, + PlaceLocation, + GripperTranslation, + PlanningOptions, + Constraints, + OrientationConstraint, +) from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from visualization_msgs.msg import Marker from grasplan.msg import PlaceObjectAction, PlaceObjectResult from moveit_msgs.msg import MoveItErrorCodes from pose_selector.srv import GetPoses -from visualization_msgs.msg import Marker, MarkerArray +from visualization_msgs.msg import MarkerArray from typing import List from std_msgs.msg import Header -class PlaceTools(): + +class PlaceTools: def __init__(self, action_server_required=True): self.global_reference_frame = rospy.get_param('~global_reference_frame', 'map') self.arm_pose_with_objs_in_fov = rospy.get_param('~arm_pose_with_objs_in_fov', 'observe100cm_right') - self.timeout = rospy.get_param('~timeout', 50.0) # in seconds + self.timeout = rospy.get_param('~timeout', 50.0) # in seconds self.min_dist = rospy.get_param('~min_dist', 0.2) self.ignore_min_dist_list = rospy.get_param('~ignore_min_dist_list', ['foo_obj']) self.group_name = rospy.get_param('~group_name', 'arm') self.arm_name = rospy.get_param('~arm_name', 'ur5') self.gripper_joint_names = rospy.get_param('~gripper_joint_names') self.gripper_joint_efforts = rospy.get_param('~gripper_joint_efforts') - self.place_object_server_name = rospy.get_param('~place_object_server_name', 'place') # /mobipick/place + self.place_object_server_name = rospy.get_param('~place_object_server_name', 'place') # /mobipick/place self.gripper_release_distance = rospy.get_param('~gripper_release_distance', 0.1) self.planning_time = rospy.get_param('~planning_time', 20.0) arm_goal_tolerance = rospy.get_param('~arm_goal_tolerance', 0.01) @@ -53,12 +67,23 @@ def __init__(self, action_server_required=True): self.marker_array_pub = rospy.Publisher('/place_pose_selector_objects', MarkerArray, queue_size=1) # service clients - place_pose_selector_activate_srv_name = rospy.get_param('~place_pose_selector_activate_srv_name', '/place_pose_selector_activate') - place_pose_selector_clear_srv_name = rospy.get_param('~place_pose_selector_clear_srv_name', '/place_pose_selector_clear') - pick_pose_selector_activate_srv_name = rospy.get_param('~pick_pose_selector_activate_srv_name', '/pick_pose_selector_activate') - pick_pose_selector_get_all_poses_srv_name = rospy.get_param('~pick_pose_selector_get_all_poses_srv_name', '/pose_selector_get_all') - rospy.loginfo(f'waiting for pose selector services: {place_pose_selector_activate_srv_name}, {place_pose_selector_clear_srv_name}\ - {pick_pose_selector_activate_srv_name}, {pick_pose_selector_get_all_poses_srv_name}') + place_pose_selector_activate_srv_name = rospy.get_param( + '~place_pose_selector_activate_srv_name', '/place_pose_selector_activate' + ) + place_pose_selector_clear_srv_name = rospy.get_param( + '~place_pose_selector_clear_srv_name', '/place_pose_selector_clear' + ) + pick_pose_selector_activate_srv_name = rospy.get_param( + '~pick_pose_selector_activate_srv_name', '/pick_pose_selector_activate' + ) + pick_pose_selector_get_all_poses_srv_name = rospy.get_param( + '~pick_pose_selector_get_all_poses_srv_name', '/pose_selector_get_all' + ) + rospy.loginfo( + 'waiting for pose selector services: {place_pose_selector_activate_srv_name},' + ' {place_pose_selector_clear_srv_name} {pick_pose_selector_activate_srv_name},' + ' {pick_pose_selector_get_all_poses_srv_name}' + ) rospy.wait_for_service(place_pose_selector_activate_srv_name, 30.0) rospy.wait_for_service(place_pose_selector_clear_srv_name, 30.0) rospy.wait_for_service(pick_pose_selector_activate_srv_name, 30.0) @@ -67,14 +92,19 @@ def __init__(self, action_server_required=True): self.activate_place_pose_selector_srv = rospy.ServiceProxy(place_pose_selector_activate_srv_name, SetBool) self.place_pose_selector_clear_srv = rospy.ServiceProxy(place_pose_selector_clear_srv_name, Trigger) self.activate_pick_pose_selector_srv = rospy.ServiceProxy(pick_pose_selector_activate_srv_name, SetBool) - self.get_all_poses_pick_pose_selector_srv = rospy.ServiceProxy(pick_pose_selector_get_all_poses_srv_name, GetPoses) + self.get_all_poses_pick_pose_selector_srv = rospy.ServiceProxy( + pick_pose_selector_get_all_poses_srv_name, GetPoses + ) rospy.loginfo('found pose selector services') except rospy.exceptions.ROSException: - rospy.logfatal('grasplan place server could not find pose selector services in time, exiting! \n' + traceback.format_exc()) + rospy.logfatal( + 'grasplan place server could not find pose selector services in time, exiting! \n' + + traceback.format_exc() + ) rospy.signal_shutdown('fatal error') # activate place pose selector to be ready to store the place poses - resp = self.activate_place_pose_selector_srv(True) + self.activate_place_pose_selector_srv(True) # wait for moveit to become available, TODO: find a cleaner way to wait for moveit rospy.wait_for_service('move_group/planning_scene_monitor/set_parameters', 30.0) @@ -89,17 +119,21 @@ def __init__(self, action_server_required=True): self.scene = moveit_commander.PlanningSceneInterface() rospy.loginfo('found move_group action server') except RuntimeError: - rospy.logfatal('grasplan place server could not connect to Moveit in time, exiting! \n' + traceback.format_exc()) + rospy.logfatal( + 'grasplan place server could not connect to Moveit in time, exiting! \n' + traceback.format_exc() + ) rospy.signal_shutdown('fatal error') # offer action lib server for object placing if needed if action_server_required: - self.place_action_server = actionlib.SimpleActionServer('place_object', PlaceObjectAction, self.place_obj_action_callback, False) + self.place_action_server = actionlib.SimpleActionServer( + 'place_object', PlaceObjectAction, self.place_obj_action_callback, False + ) self.place_action_server.start() self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) - + def clear_place_poses_markers(self): marker_array_msg = MarkerArray() marker = Marker() @@ -145,7 +179,7 @@ def move_arm_to_posture(self, arm_posture_name): def place_obj_action_callback(self, goal): success = False - num_poses_list = [5, 25, 50] # first try 5 poses, then 25, then 50 + num_poses_list = [5, 25, 50] # first try 5 poses, then 25, then 50 override_disentangle_dont_doit = False override_observe_before_place_dont_doit = False for i, num_poses in enumerate(num_poses_list): @@ -160,9 +194,13 @@ def place_obj_action_callback(self, goal): # do not disentangle if we dont go to observe arm pose if not goal.observe_before_place: override_disentangle_dont_doit = True - if self.place_object(goal.support_surface_name, observe_before_place=goal.observe_before_place,\ - number_of_poses=num_poses, override_disentangle_dont_doit=override_disentangle_dont_doit,\ - override_observe_before_place_dont_doit=override_observe_before_place_dont_doit): + if self.place_object( + goal.support_surface_name, + observe_before_place=goal.observe_before_place, + number_of_poses=num_poses, + override_disentangle_dont_doit=override_disentangle_dont_doit, + override_observe_before_place_dont_doit=override_observe_before_place_dont_doit, + ): success = True break if success: @@ -179,16 +217,18 @@ def transform_obj_list(self, obj_list: ObjectList, target_frame_id: str) -> Obje target_pose = tf2_geometry_msgs.do_transform_pose(source_pose, tf) obj.pose = target_pose.pose else: - raise RuntimeError(f"Error while transformin obj_list, no tf from {target_frame_id} -> {obj_list.header.frame_id}") + raise RuntimeError( + f"Error while transformin obj_list, no tf from {target_frame_id} -> {obj_list.header.frame_id}" + ) obj_list.header.frame_id = target_frame_id return obj_list - + def transform_points(self, points: List[Point], target_frame_id: str, source_frame_id: str) -> List[Point]: """Transforms a list of points from a source frame to a target frame.""" if self.tf_buffer.can_transform(target_frame_id, source_frame_id, rospy.Time(0)): transformed_points = [] for point in points: - source_point = PointStamped(header=Header(frame_id=source_frame_id), point=point) + source_point = PointStamped(header=Header(frame_id=source_frame_id), point=point) tf = self.tf_buffer.lookup_transform(target_frame_id, source_frame_id, rospy.Time(0)) target_point = tf2_geometry_msgs.do_transform_point(source_point, tf) transformed_points.append(target_point.point) @@ -200,12 +240,21 @@ def add_planning_scene_objects(self, planning_scene) -> None: """Adds all objects in the planning scene to the local tf buffer as static transforms.""" for obj_name in planning_scene.get_known_object_names(): obj = planning_scene.get_objects([obj_name])[obj_name] - tf = TransformStamped(header=Header(frame_id="map"), child_frame_id=obj_name, - transform=Transform(translation=obj.pose.position, rotation=obj.pose.orientation)) + tf = TransformStamped( + header=Header(frame_id="map"), + child_frame_id=obj_name, + transform=Transform(translation=obj.pose.position, rotation=obj.pose.orientation), + ) self.tf_buffer.set_transform_static(tf, "grasplan") - def place_object(self, support_object, observe_before_place=False, number_of_poses=5,\ - override_disentangle_dont_doit=False, override_observe_before_place_dont_doit=False): + def place_object( + self, + support_object, + observe_before_place=False, + number_of_poses=5, + override_disentangle_dont_doit=False, + override_observe_before_place_dont_doit=False, + ): ''' create action lib client and call moveit place action server ''' @@ -218,7 +267,9 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos # deduce object_to_be_placed by querying which object the gripper currently has attached to its gripper object_to_be_placed = list(self.scene.get_attached_objects().keys())[0] - rospy.loginfo(f'received request to place the object that the robot is currently holding : {object_to_be_placed}') + rospy.loginfo( + f'received request to place the object that the robot is currently holding : {object_to_be_placed}' + ) # TODO: find a better place for adding the static transforms, it shouldn't be done for each place self.add_planning_scene_objects(self.scene) @@ -231,9 +282,9 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos # optionally find free space in table: look at table, update planning scene self.move_arm_to_posture(self.arm_pose_with_objs_in_fov) # activate pick pose selector to observe table - resp = self.activate_pick_pose_selector_srv(True) - rospy.sleep(0.5) # give some time to observe - resp = self.activate_pick_pose_selector_srv(False) + self.activate_pick_pose_selector_srv(True) + rospy.sleep(0.5) # give some time to observe + self.activate_pick_pose_selector_srv(False) self.add_objs_to_planning_scene() action_client = actionlib.SimpleActionClient(self.place_object_server_name, PlaceAction) @@ -246,14 +297,25 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos plane = adjust_plane(plane, -0.15, -0.15) # publish plane as marker for visualization purposes - self.plane_vis_pub.publish(make_plane_marker_msg(self.global_reference_frame, self.transform_points(plane, self.global_reference_frame, support_object))) - + self.plane_vis_pub.publish( + make_plane_marker_msg( + self.global_reference_frame, self.transform_points(plane, self.global_reference_frame, support_object) + ) + ) + # generate random places within the plane object_class_tbp = separate_object_class_from_id(object_to_be_placed)[0] - local_place_poses = gen_place_poses_from_plane(object_class_tbp, support_object, plane, self.scene, \ - frame_id=support_object, number_of_poses=number_of_poses, \ - min_dist=self.min_dist, ignore_min_dist_list=self.ignore_min_dist_list) - + local_place_poses = gen_place_poses_from_plane( + object_class_tbp, + support_object, + plane, + self.scene, + frame_id=support_object, + number_of_poses=number_of_poses, + min_dist=self.min_dist, + ignore_min_dist_list=self.ignore_min_dist_list, + ) + global_place_poses = self.transform_obj_list(local_place_poses, self.global_reference_frame) self.place_poses_pub.publish(global_place_poses) @@ -264,9 +326,9 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos if action_client.wait_for_server(timeout=rospy.Duration.from_sec(2.0)): rospy.loginfo(f'found {self.place_object_server_name} action server') - goal = self.make_place_goal_msg(object_to_be_placed, support_object, \ - global_place_poses, \ - use_path_constraints=self.use_path_constraints) + goal = self.make_place_goal_msg( + object_to_be_placed, support_object, global_place_poses, use_path_constraints=self.use_path_constraints + ) # allow disentangle to happen only on first place attempt, no need to do it every time if not override_disentangle_dont_doit: @@ -283,44 +345,44 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos result = action_client.get_result() # rospy.loginfo(f'{self.place_object_server_name} is done with execution, resuĺt was = "{result}"') - ## ------ result handling + # # ------ result handling - ## The result of the place attempt + # # The result of the place attempt # MoveItErrorCodes error_code - ## The full starting state of the robot at the start of the trajectory + # # The full starting state of the robot at the start of the trajectory # RobotState trajectory_start - ## The trajectory that moved group produced for execution + # # The trajectory that moved group produced for execution # RobotTrajectory[] trajectory_stages # string[] trajectory_descriptions - ## The successful place location, if any + # # The successful place location, if any # PlaceLocation place_location - ## The amount of time in seconds it took to complete the plan + # # The amount of time in seconds it took to complete the plan # float64 planning_time # --- # how to know if place was successful from result? - # if result.success == True: - # rospy.loginfo(f'Succesfully placed {object_to_be_placed}') + # if result.success: + # rospy.loginfo(f'Succesfully placed {object_to_be_placed}') # else: - # rospy.logerr(f'Failed to place {object_to_be_placed}') + # rospy.logerr(f'Failed to place {object_to_be_placed}') # --- # handle moveit pick result if result.error_code.val == MoveItErrorCodes.SUCCESS: - rospy.loginfo(f'Successfully placed object') + rospy.loginfo('Successfully placed object') self.place_pose_selector_clear_srv() # clear possible place poses markers in rviz self.clear_place_poses_markers() return True else: - rospy.logerr(f'place object failed') + rospy.logerr('place object failed') self.place_pose_selector_clear_srv() # clear possible place poses markers in rviz self.clear_place_poses_markers() @@ -331,7 +393,7 @@ def place_object(self, support_object, observe_before_place=False, number_of_pos return False return False - def make_constraints_msg(self): # TODO: not yet tested on mobipick + def make_constraints_msg(self): # TODO: not yet tested on mobipick constraints_msg = Constraints() constraints_msg.name = 'keep_object_upright' @@ -356,7 +418,9 @@ def make_constraints_msg(self): # TODO: not yet tested on mobipick return constraints_msg - def make_place_goal_msg(self, object_to_be_placed, support_object, place_poses_as_object_list_msg, use_path_constraints): + def make_place_goal_msg( + self, object_to_be_placed, support_object, place_poses_as_object_list_msg, use_path_constraints + ): ''' fill place action lib goal, see: https://github.com/ros-planning/moveit_msgs/blob/master/action/Place.action ''' @@ -374,8 +438,8 @@ def make_place_goal_msg(self, object_to_be_placed, support_object, place_poses_a # NOTE: multiple place locations are possible to be defined, we just define 1 for now # --- place_locations = [] - frame_id = place_poses_as_object_list_msg.header.frame_id # 'mobipick/base_link' - object_class = separate_object_class_from_id(object_to_be_placed)[0] + frame_id = place_poses_as_object_list_msg.header.frame_id # 'mobipick/base_link' + # object_class = separate_object_class_from_id(object_to_be_placed)[0] # for obj in self.gen_place_poses(object_class, frame_id=frame_id).objects: for obj in place_poses_as_object_list_msg.objects: pose_stamped_msg = PoseStamped() @@ -406,7 +470,7 @@ def make_place_goal_msg(self, object_to_be_placed, support_object, place_poses_a # Optional constraints to be imposed on every point in the motion plan # Constraints path_constraints if use_path_constraints: - goal.path_constraints = self.make_constraints_msg() # add orientation constraints + goal.path_constraints = self.make_constraints_msg() # add orientation constraints # The name of the motion planner to use. If no name is specified, # a default motion planner will be used @@ -436,7 +500,7 @@ def make_planning_options_msg(self): # The diff to consider for the planning scene (optional) # PlanningScene planning_scene_diff - # planning_options_msg.planning_scene_diff = + # planning_options_msg.planning_scene_diff = # NOTE: It's important to set is_diff = True, otherwise MoveIt will # overwrite its planning scene with this one (empty), thereby ignoring # collisions with e.g. the octomap @@ -552,7 +616,9 @@ def make_gripper_trajectory_msg(self, gripper_actuation_distance): trajectory.points.append(trajectory_point) return trajectory - def make_gripper_translation_msg(self, frame_id, distance, vector_x=0.0, vector_y=0.0, vector_z=0.0, min_distance=0.1): + def make_gripper_translation_msg( + self, frame_id, distance, vector_x=0.0, vector_y=0.0, vector_z=0.0, min_distance=0.1 + ): ''' see: https://github.com/ros-planning/moveit_msgs/blob/master/msg/GripperTranslation.msg ''' @@ -588,7 +654,8 @@ def start_place_node(self): # shutdown moveit cpp interface before exit # moveit_commander.roscpp_shutdown() -if __name__=='__main__': + +if __name__ == '__main__': rospy.init_node('place_object_node', anonymous=False) place = PlaceTools() place.start_place_node() diff --git a/src/grasplan/pose_generator.py b/src/grasplan/pose_generator.py index e05babd..b9322af 100755 --- a/src/grasplan/pose_generator.py +++ b/src/grasplan/pose_generator.py @@ -7,10 +7,12 @@ from geometry_msgs.msg import PoseArray, Pose, PoseStamped -class PoseGenerator(): + +class PoseGenerator: ''' Receive a pose and generate multiple ones with different orientations ''' + def __init__(self): # parameters @@ -49,9 +51,9 @@ def generate_angles(self, start=0.0, end=2.0, step=0.1): result = [3.0, 4.0, 2.0, 5.0, 1.0, 0.0] ''' my_list = [] - if abs(start - end) < 0.000001: # if start == end + if abs(start - end) < 0.000001: # if start == end return [start] - assert(start < end) + assert start < end temp_number = start my_list.append(start) while temp_number < end: @@ -65,9 +67,9 @@ def generate_angles(self, start=0.0, end=2.0, step=0.1): def spherical_sampling(self, grasp_type, original_pose, offset_vector): # offset to object tf tf_offset_to_object = tf.transformations.euler_matrix(0, 0, 0) - tf_offset_to_object[0][3] = offset_vector[0] # x - tf_offset_to_object[1][3] = offset_vector[1] # y - tf_offset_to_object[2][3] = offset_vector[2] # z + tf_offset_to_object[0][3] = offset_vector[0] # x + tf_offset_to_object[1][3] = offset_vector[1] # y + tf_offset_to_object[2][3] = offset_vector[2] # z # object to world tf rot = [] @@ -77,9 +79,9 @@ def spherical_sampling(self, grasp_type, original_pose, offset_vector): rot.append(original_pose.pose.orientation.w) euler_rot = tf.transformations.euler_from_quaternion(rot) tf_object_to_world = tf.transformations.euler_matrix(euler_rot[0], euler_rot[1], euler_rot[2]) - tf_object_to_world[0][3] = original_pose.pose.position.x # x - tf_object_to_world[1][3] = original_pose.pose.position.y # y - tf_object_to_world[2][3] = original_pose.pose.position.z # z + tf_object_to_world[0][3] = original_pose.pose.position.x # x + tf_object_to_world[1][3] = original_pose.pose.position.y # y + tf_object_to_world[2][3] = original_pose.pose.position.z # z # prepare pose array msg pose_array_msg = PoseArray() @@ -103,12 +105,16 @@ def spherical_sampling(self, grasp_type, original_pose, offset_vector): for pitch in self.generate_angles(start=pitch_start, end=pitch_end, step=pitch_step): for yaw in self.generate_angles(start=yaw_start, end=yaw_end, step=yaw_step): # apply rotations to identity matrix - nm = tf.transformations.euler_matrix(roll, pitch, yaw) # rpy + nm = tf.transformations.euler_matrix(roll, pitch, yaw) # rpy # transform back to world reference frame processed_pose_in_world_rf_m = np.dot(tf_object_to_world, np.dot(tf_offset_to_object, nm)) proll, ppitch, pyaw = tf.transformations.euler_from_matrix(processed_pose_in_world_rf_m) - position = (processed_pose_in_world_rf_m[0][3], processed_pose_in_world_rf_m[1][3], processed_pose_in_world_rf_m[2][3]) + position = ( + processed_pose_in_world_rf_m[0][3], + processed_pose_in_world_rf_m[1][3], + processed_pose_in_world_rf_m[2][3], + ) q_orientation = tf.transformations.quaternion_from_euler(proll, ppitch, pyaw) # pack elements into pose @@ -127,7 +133,8 @@ def spherical_sampling(self, grasp_type, original_pose, offset_vector): def publish_pose_array_msg(self, pose_array_msg): self.pose_array_pub.publish(pose_array_msg) -if __name__=='__main__': + +if __name__ == '__main__': # example of how to use this node, in practice it is used as a library rospy.init_node('pose_generator_node', anonymous=False) pose_generator = PoseGenerator() diff --git a/src/grasplan/rqt_grasplan/grasps.py b/src/grasplan/rqt_grasplan/grasps.py index 905ab0a..9b72e56 100644 --- a/src/grasplan/rqt_grasplan/grasps.py +++ b/src/grasplan/rqt_grasplan/grasps.py @@ -1,10 +1,9 @@ -#!/usr/bin/env python3 - import tf import copy import numpy as np from geometry_msgs.msg import Pose, PoseArray + class GraspEditorState: def __init__(self): self.__grasps = None @@ -16,7 +15,7 @@ def set_grasps(self, grasps): self.__grasps = [] else: assert isinstance(grasps[0], Pose) - self.__grasps = copy.deepcopy(grasps) # deepcopy is essential here! + self.__grasps = copy.deepcopy(grasps) # deepcopy is essential here! def get_grasps(self): return self.__grasps @@ -28,17 +27,18 @@ def set_selected_grasp_index(self, selected_grasp_index): def get_selected_grasp_index(self): return self.__selected_grasp_index + class Grasps: def __init__(self, reference_frame='object', history_buffer_size=100): self.reference_frame = reference_frame - self.history_buffer_size = history_buffer_size # the maximum number of times you can perform undo + self.history_buffer_size = history_buffer_size # the maximum number of times you can perform undo self.grasp_history = None self.undo_index = None self.__pause_history = None self.grasps_as_pose_array = None # flag used to highlight a grasp in green color, set to -1 to not highlight any pose in particular self.selected_grasp_index = None - self.__init() # must be called at the end of this constructor + self.__init() # must be called at the end of this constructor def __init(self): self.grasps_as_pose_array = PoseArray() @@ -87,10 +87,10 @@ def add_grasps(self, pose_array_grasps): for grasp in pose_array_grasps.poses: self.add_grasp(grasp) - def rotate_grasp(self, grasp, roll=0., pitch=0., yaw=0.): + def rotate_grasp(self, grasp, roll=0.0, pitch=0.0, yaw=0.0): return self.transform_grasp(grasp, angular_rpy=[roll, pitch, yaw]) - def transform_grasp(self, grasp, linear=[0., 0., 0.], angular_rpy=[0., 0., 0.]): + def transform_grasp(self, grasp, linear=[0.0, 0.0, 0.0], angular_rpy=[0.0, 0.0, 0.0]): ''' input: geometry_msgs/Pose (grasp) and the incremental transform that you want to apply output: this function does not modify the input grasp by reference @@ -102,8 +102,14 @@ def transform_grasp(self, grasp, linear=[0., 0., 0.], angular_rpy=[0., 0., 0.]): assert isinstance(linear, list) assert isinstance(angular_rpy, list) derived_grasp = copy.deepcopy(grasp) - q_orig = np.array([derived_grasp.orientation.x, derived_grasp.orientation.y,\ - derived_grasp.orientation.z, derived_grasp.orientation.w]) + q_orig = np.array( + [ + derived_grasp.orientation.x, + derived_grasp.orientation.y, + derived_grasp.orientation.z, + derived_grasp.orientation.w, + ] + ) angular_q = tf.transformations.quaternion_from_euler(angular_rpy[0], angular_rpy[1], angular_rpy[2]) q_new = tf.transformations.quaternion_multiply(angular_q, q_orig) derived_grasp.position.x += linear[0] @@ -115,7 +121,7 @@ def transform_grasp(self, grasp, linear=[0., 0., 0.], angular_rpy=[0., 0., 0.]): derived_grasp.orientation.w = q_new[3] return derived_grasp - def rotate_grasps(self, grasps, roll=0., pitch=0., yaw=0., replace=False): + def rotate_grasps(self, grasps, roll=0.0, pitch=0.0, yaw=0.0, replace=False): self.transform_grasps(grasps, angular_rpy=[roll, pitch, yaw], replace=replace) def find_grasp_index(self, grasp): @@ -131,9 +137,9 @@ def find_grasp_index(self, grasp): return i return -1 - def transform_grasps(self, grasps, linear=[0., 0., 0.], angular_rpy=[0., 0., 0.], replace=False): + def transform_grasps(self, grasps, linear=[0.0, 0.0, 0.0], angular_rpy=[0.0, 0.0, 0.0], replace=False): assert isinstance(grasps, list) - self.pause_history() # for undo to work on all pattern poses we pause history + self.pause_history() # for undo to work on all pattern poses we pause history static_grasps = copy.deepcopy(grasps) for grasp in static_grasps: assert isinstance(grasp, Pose) @@ -142,17 +148,19 @@ def transform_grasps(self, grasps, linear=[0., 0., 0.], angular_rpy=[0., 0., 0.] if replace: grasp_index = self.find_grasp_index(grasp) if grasp_index == -1: - raise ValueError('Could not find grasp index while trying to replace after transform, this should not happen.') + raise ValueError( + 'Could not find grasp index while trying to replace after transform, this should not happen.' + ) self.replace_grasp_by_index(grasp_index, derived_grasp) else: self.add_grasp(derived_grasp) - self.unpause_history() # for undo to work on all pattern poses we unpause history + self.unpause_history() # for undo to work on all pattern poses we unpause history self.add_state_to_history() - def rotate_selected_grasps(self, roll=0., pitch=0., yaw=0., replace=False): + def rotate_selected_grasps(self, roll=0.0, pitch=0.0, yaw=0.0, replace=False): return self.transform_selected_grasps(angular_rpy=[roll, pitch, yaw], replace=replace) - def transform_selected_grasps(self, linear=[0., 0., 0.], angular_rpy=[0., 0., 0.], replace=False): + def transform_selected_grasps(self, linear=[0.0, 0.0, 0.0], angular_rpy=[0.0, 0.0, 0.0], replace=False): if self.no_grasp_is_selected(): return False grasps = self.get_selected_grasps() @@ -240,7 +248,7 @@ def get_selected_grasps(self): return [self.get_selected_grasp()] elif self.no_grasp_is_selected(): return [] - elif self.selected_grasp_index == -10: # all grasps are selected + elif self.selected_grasp_index == -10: # all grasps are selected return self.get_grasps_as_pose_list() def get_selected_grasp_index(self): diff --git a/src/grasplan/rqt_grasplan/grasps_test.py b/src/grasplan/rqt_grasplan/grasps_test.py old mode 100644 new mode 100755 index 38e0ecd..df574b6 --- a/src/grasplan/rqt_grasplan/grasps_test.py +++ b/src/grasplan/rqt_grasplan/grasps_test.py @@ -1,7 +1,5 @@ #!/usr/bin/env python3 -PKG = 'test_roslaunch' - import tf import math import copy @@ -13,8 +11,10 @@ import logging -class TestGrasps(unittest.TestCase): +PKG = 'test_roslaunch' + +class TestGrasps(unittest.TestCase): def get_identity_grasp_msg(self): grasp = Pose() grasp.position.x = 0.0 @@ -27,10 +27,7 @@ def get_identity_grasp_msg(self): return copy.deepcopy(grasp) def pose_to_quaternion_list(self, pose): - q_list = [pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w] + q_list = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] return q_list def get_grasps_object(self): @@ -81,7 +78,7 @@ def test_rotate_grasp_yaw(self): q = self.pose_to_quaternion_list(grasp) # quaternion with a 90.0 degree yaw rotation desired_q = [0.0, 0.0, 0.7071067811865475, 0.7071067811865476] - self.assertEquals(np.allclose(desired_q, q), True) + self.assertEquals(np.allclose(desired_q, q), True) def test_rotate_selected_grasps_replace_true(self): g = self.get_grasps_object() @@ -125,10 +122,9 @@ def test_find_grasp_index(self): self.assertEquals(grasp_index, 0) def test_replace_grasp_by_index(self): - log= logging.getLogger( "TestGrasps.test_replace_grasp_by_index" ) + log = logging.getLogger("TestGrasps.test_replace_grasp_by_index") log.info('hola') g = self.get_grasps_object() - grasp = g.get_grasp_by_index(0) identity_grasp = self.get_identity_grasp_msg() grasp_index = g.grasps_as_pose_array.poses.index(identity_grasp) self.assertEquals(grasp_index, 0) @@ -147,15 +143,15 @@ def test_replace_grasp_by_index(self): n_grasp.position.y = 0.0 n_grasp.position.z = 0.0 # --- - n_grasp.orientation.x = q[0] # passes test + n_grasp.orientation.x = q[0] # passes test n_grasp.orientation.y = q[1] n_grasp.orientation.z = q[2] n_grasp.orientation.w = q[3] # --- - #n_grasp.orientation.x = desired_q[0] # fails test - #n_grasp.orientation.y = desired_q[1] - #n_grasp.orientation.z = desired_q[2] - #n_grasp.orientation.w = desired_q[3] + # n_grasp.orientation.x = desired_q[0] # fails test + # n_grasp.orientation.y = desired_q[1] + # n_grasp.orientation.z = desired_q[2] + # n_grasp.orientation.w = desired_q[3] # --- self.assertEquals(desired_q, list(q)) g.add_grasp(g.transform_grasp(self.get_identity_grasp_msg(), angular_rpy=[0.0, math.radians(90.0), 0.0])) @@ -179,9 +175,11 @@ def test_undo_apply_transform(self): g.transform_selected_grasps(angular_rpy=[0.0, math.radians(45.0), 0.0], replace=True) g.undo() grasp = g.get_selected_grasp() - q = [0.0, 0.3826834323650898, 0.0, 0.9238795325112867] # q corresponds to a 45 deg rotation around y axis + q = [0.0, 0.3826834323650898, 0.0, 0.9238795325112867] # q corresponds to a 45 deg rotation around y axis self.assertEquals(self.pose_to_quaternion_list(grasp), q) + if __name__ == '__main__': import rostest + rostest.rosrun(PKG, 'test_g', TestGrasps) diff --git a/src/grasplan/rqt_grasplan/rqt_grasplan.py b/src/grasplan/rqt_grasplan/rqt_grasplan.py index d553746..58f9548 100644 --- a/src/grasplan/rqt_grasplan/rqt_grasplan.py +++ b/src/grasplan/rqt_grasplan/rqt_grasplan.py @@ -1,5 +1,3 @@ -#!/usr/bin/env python3 - import os import tf import math @@ -17,11 +15,13 @@ from std_msgs.msg import Int8, String from geometry_msgs.msg import Pose, PoseArray, PoseStamped + class OpenFileDialog(QWidget): ''' allow the user to select a different yaml file with a button, this will open a dialog to select and open a yaml file ''' + def __init__(self, initial_path=None): super().__init__() left, top, width, height = 10, 10, 640, 480 @@ -36,19 +36,21 @@ def openFileNameDialog(self, save_file_name_dialog=False): else: initial_path = self.initial_path if save_file_name_dialog: - fileName, _ = QFileDialog.getSaveFileName(self, 'Select grasps yaml file',\ - initial_path, 'Yaml Files (*.yaml)', options=options) + fileName, _ = QFileDialog.getSaveFileName( + self, 'Select grasps yaml file', initial_path, 'Yaml Files (*.yaml)', options=options + ) else: - fileName, _ = QFileDialog.getOpenFileName(self, 'Select grasps yaml file',\ - initial_path, 'Yaml Files (*.yaml)', options=options) + fileName, _ = QFileDialog.getOpenFileName( + self, 'Select grasps yaml file', initial_path, 'Yaml Files (*.yaml)', options=options + ) if fileName: return fileName def saveFileNameDialog(self): return self.openFileNameDialog(save_file_name_dialog=True) -class RqtGrasplan(Plugin): +class RqtGrasplan(Plugin): def __init__(self, context): super(RqtGrasplan, self).__init__(context) rospy.loginfo('Initializing grasplan rqt, have a happy grasp editing !') @@ -70,10 +72,10 @@ def __init__(self, context): self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) # variables - self.grasps = Grasps() # stores all grasps + self.grasps = Grasps() # stores all grasps self.grasps_yaml_path = None self.object_class = None - self.tab = ' ' # used in save function + self.tab = ' ' # used in save function self.global_reference_frame = 'object' # publications @@ -89,7 +91,9 @@ def __init__(self, context): # set object name to textbox self._widget.txtFileObjectName.setText(self.object_class) if rospy.has_param('~grasps_yaml_path'): - self.grasps_yaml_path = rospy.get_param('~grasps_yaml_path') + f'/handcoded_grasp_planner_{self.object_class}.yaml' + self.grasps_yaml_path = ( + rospy.get_param('~grasps_yaml_path') + f'/handcoded_grasp_planner_{self.object_class}.yaml' + ) self.grasps.add_grasps(self.load_grasps_from_yaml(self.object_class, self.grasps_yaml_path)) else: rospy.logwarn('object name parameter is set but grasps_yaml_path param is missing, is this correct?') @@ -101,7 +105,7 @@ def __init__(self, context): # visualize grasps at startup self.publish_grasps() - ## make a connection between the qt objects and this class methods + # make a connection between the qt objects and this class methods self._widget.cmdFilePrintG.clicked.connect(self.handle_file_print_grasps_button) self._widget.cmdFileLoadG.clicked.connect(self.handle_file_load_grasps_button) self._widget.cmdFileSaveG.clicked.connect(self.handle_file_save_grasps_button) @@ -135,7 +139,7 @@ def log_error(self, error_msg): msg.exec_() rospy.logerr(error_msg) - def list_to_pose_msg(self, linear=[0,0,0], angular_q=[0,0,0,1]): + def list_to_pose_msg(self, linear=[0, 0, 0], angular_q=[0, 0, 0, 1]): ''' build a pose msg from input lists return the pose msg @@ -150,7 +154,7 @@ def list_to_pose_msg(self, linear=[0,0,0], angular_q=[0,0,0,1]): pose_msg.orientation.w = angular_q[3] return pose_msg - def list_to_pose_stamped_msg(self, linear=[0,0,0], angular_q=[0,0,0,1]): + def list_to_pose_stamped_msg(self, linear=[0, 0, 0], angular_q=[0, 0, 0, 1]): ''' build a pose stamped msg from input lists return the pose stamped msg @@ -166,7 +170,7 @@ def list_to_pose_stamped_msg(self, linear=[0,0,0], angular_q=[0,0,0,1]): pose_stamped_msg.pose.orientation.w = angular_q[3] return pose_stamped_msg - def publish_test_pose(self, linear=[0,0,0], angular_q=[0,0,0,1]): + def publish_test_pose(self, linear=[0, 0, 0], angular_q=[0, 0, 0, 1]): ''' build pose stamped msg from input lists publish to test topic to visualize in rviz @@ -187,19 +191,20 @@ def write_grasps_to_yaml_file(self, grasps, object_name, grasps_yaml_path): translation_str = f'{tab}{tab}{tab}translation: [{linear[0]:.6f}, {linear[1]:.6f}, {linear[2]:.6f}]' grasp_stream_list.append(translation_str) # rotation - rotation_str = f'{tab}{tab}{tab}rotation: [{angular_q[0]:.6f}, {angular_q[1]:.6f}, {angular_q[2]:.6f}, {angular_q[3]:.6f}]' + rotation_str = f'{tab}{tab}{tab}rotation: [{angular_q[0]:.6f}, {angular_q[1]:.6f}, {angular_q[2]:.6f},' + ' {angular_q[3]:.6f}]' grasp_stream_list.append(rotation_str) rospy.loginfo(f'writing grasps to file: {grasps_yaml_path}') - f = open(grasps_yaml_path,'w+') + f = open(grasps_yaml_path, 'w+') for string in grasp_stream_list: f.write(string + '\n') f.close() def handle_file_save_grasps_button(self): rospy.loginfo('saving grasps!') - self.write_grasps_to_yaml_file(self.grasps.get_grasps_as_pose_list(),\ - self._widget.txtFileObjectName.toPlainText(),\ - self.grasps_yaml_path) + self.write_grasps_to_yaml_file( + self.grasps.get_grasps_as_pose_list(), self._widget.txtFileObjectName.toPlainText(), self.grasps_yaml_path + ) def handle_file_save_grasps_as_button(self): rospy.loginfo('saving grasps as!') @@ -275,9 +280,11 @@ def handle_file_print_grasps_button(self): ''' rospy.loginfo('print!') for grasp in self.grasps.get_grasps_as_pose_list(): - print(f'{self.tab}-\n{self.tab} translation: [{grasp.position.x},{grasp.position.y},{grasp.position.z}]\n' +\ - f'{self.tab} rotation: [{grasp.orientation.x},{grasp.orientation.y},\ - {grasp.orientation.z},{grasp.orientation.w}]') + print( + f'{self.tab}-\n{self.tab} translation: [{grasp.position.x},{grasp.position.y},{grasp.position.z}]\n' + + f'{self.tab} rotation: [{grasp.orientation.x},{grasp.orientation.y},\ + {grasp.orientation.z},{grasp.orientation.w}]' + ) def load_grasps_from_yaml(self, object_class, grasps_yaml_path): rospy.loginfo(f'reloading grasps from file: {grasps_yaml_path}') @@ -292,8 +299,10 @@ def load_grasps_from_yaml(self, object_class, grasps_yaml_path): if grasps_dic is None: return None # load grasps from param server - if not object_class in grasps_dic: - self.log_error(f'Object "{object_class}" not found in dictionary, check input yaml file: {grasps_yaml_path}') + if object_class not in grasps_dic: + self.log_error( + f'Object "{object_class}" not found in dictionary, check input yaml file: {grasps_yaml_path}' + ) return None else: rospy.loginfo(f'loading {object_class} grasps from yaml file: {grasps_yaml_path}') @@ -333,13 +342,13 @@ def write_linear_to_tf_textbox(self, linear): def convert_rpy_rad_to_deg(self, rpy_in_rad): rpy_in_deg = [] - for i in range(3): # 0, 1, 2 + for i in range(3): # 0, 1, 2 rpy_in_deg.append(math.degrees(rpy_in_rad[i])) return rpy_in_deg def convert_rpy_deg_to_rad(self, rpy_in_deg): rpy_in_rad = [] - for i in range(3): # 0, 1, 2 + for i in range(3): # 0, 1, 2 rpy_in_rad.append(math.radians(rpy_in_deg[i])) return rpy_in_rad @@ -354,11 +363,15 @@ def handle_grasp_s_select_button(self): if self._widget.chkTransformLoadSelected.isChecked(): if self.grasps.single_grasp_is_selected(): selected_grasp = self.grasps.get_selected_grasp() - self.write_linear_to_tf_textbox([selected_grasp.position.x,\ - selected_grasp.position.y,\ - selected_grasp.position.z]) - angular_q = [selected_grasp.orientation.x, selected_grasp.orientation.y,\ - selected_grasp.orientation.z, selected_grasp.orientation.w] + self.write_linear_to_tf_textbox( + [selected_grasp.position.x, selected_grasp.position.y, selected_grasp.position.z] + ) + angular_q = [ + selected_grasp.orientation.x, + selected_grasp.orientation.y, + selected_grasp.orientation.z, + selected_grasp.orientation.w, + ] self.write_q_to_tf_textbox(angular_q) angular_rpy = list(tf.transformations.euler_from_quaternion(angular_q)) if self._widget.optTransformUnitsDeg.isChecked(): @@ -382,8 +395,10 @@ def handle_grasp_s_delete_button(self): rospy.loginfo('deleting all grasps!') self.grasps.remove_all_grasps() else: - rospy.logwarn('deleting all grasps but leaving grasp #0,\ - if you want to remove it click delete again') + rospy.logwarn( + 'deleting all grasps but leaving grasp #0,\ + if you want to remove it click delete again' + ) self.grasps.remove_all_but_one_grasp() elif self.update_selected_grasp(): self.grasps.remove_selected_grasp() @@ -459,16 +474,22 @@ def handle_grasp_s_unselect_button(self): self.publish_grasps() def read_transform(self, apply_rpy_to_q=False): - linear = [float(self._widget.txtTransformLinearX.toPlainText()),\ - float(self._widget.txtTransformLinearY.toPlainText()),\ - float(self._widget.txtTransformLinearZ.toPlainText())] - angular_rpy = [float(self._widget.txtTransformAngularR.toPlainText()),\ - float(self._widget.txtTransformAngularP.toPlainText()),\ - float(self._widget.txtTransformAngularY.toPlainText())] - angular_q = [float(self._widget.txtTransformAngularQx.toPlainText()),\ - float(self._widget.txtTransformAngularQy.toPlainText()),\ - float(self._widget.txtTransformAngularQz.toPlainText()),\ - float(self._widget.txtTransformAngularQw.toPlainText())] + linear = [ + float(self._widget.txtTransformLinearX.toPlainText()), + float(self._widget.txtTransformLinearY.toPlainText()), + float(self._widget.txtTransformLinearZ.toPlainText()), + ] + angular_rpy = [ + float(self._widget.txtTransformAngularR.toPlainText()), + float(self._widget.txtTransformAngularP.toPlainText()), + float(self._widget.txtTransformAngularY.toPlainText()), + ] + angular_q = [ + float(self._widget.txtTransformAngularQx.toPlainText()), + float(self._widget.txtTransformAngularQy.toPlainText()), + float(self._widget.txtTransformAngularQz.toPlainText()), + float(self._widget.txtTransformAngularQw.toPlainText()), + ] if self._widget.optTransformUnitsRad.isChecked(): # rpy values are in radians if angular_rpy[0] > math.pi or angular_rpy[1] > math.pi or angular_rpy[2] > math.pi: diff --git a/src/grasplan/rqt_planning_scene/rosbag_interval_pub.py b/src/grasplan/rqt_planning_scene/rosbag_interval_pub.py index 97f8991..5af6cf4 100755 --- a/src/grasplan/rqt_planning_scene/rosbag_interval_pub.py +++ b/src/grasplan/rqt_planning_scene/rosbag_interval_pub.py @@ -13,7 +13,21 @@ import yaml import subprocess + class RosbagIntervalPub: + ''' + # Usage example: + + if __name__ == '__main__': + rospy.init_node('rosbag_utils_tester') + bag_path = '/home/user/my_bag.bag' + rip = RosbagIntervalPub(bag_path) + + # play all msgs within 45 and 50 percent of the rosbag + # e.g. rosbag duration = 100.0 secs, will play all msgs between 45.0 and 50.0 secs + rip.pub_within_percentage_interval(45.0, 50.0) + ''' + def __init__(self, bag_path): self.bag = rosbag.Bag(bag_path) self.bag_path = bag_path @@ -34,7 +48,15 @@ def pub_within_interval(self, start_time, end_time): start_time, end_time in seconds ''' duration = end_time - start_time - subprocess_args = ['rosbag', 'play', str(self.bag_path), '--start', str(start_time - self.bag_start), '--duration', str(duration)] + subprocess_args = [ + 'rosbag', + 'play', + str(self.bag_path), + '--start', + str(start_time - self.bag_start), + '--duration', + str(duration), + ] cmd_str = '' for arg in subprocess_args: cmd_str += arg + ' ' @@ -50,15 +72,3 @@ def pub_within_percentage_interval(self, percentage_start, percentage_end): end_time = percentage_end * self.bag_duration / 100.0 + self.bag_start rospy.loginfo(f'playing msgs within time interval : {start_time} - {end_time} [sec] ') self.pub_within_interval(start_time, end_time) - -# Usage example: -''' -if __name__ == '__main__': - rospy.init_node('rosbag_utils_tester') - bag_path = '/home/user/my_bag.bag' - rip = RosbagIntervalPub(bag_path) - - # play all msgs within 45 and 50 percent of the rosbag - # e.g. rosbag duration = 100.0 secs, will play all msgs between 45.0 and 50.0 secs - rip.pub_within_percentage_interval(45.0, 50.0) -''' diff --git a/src/grasplan/rqt_planning_scene/rqt_planning_scene.py b/src/grasplan/rqt_planning_scene/rqt_planning_scene.py index dbb4577..5601e36 100644 --- a/src/grasplan/rqt_planning_scene/rqt_planning_scene.py +++ b/src/grasplan/rqt_planning_scene/rqt_planning_scene.py @@ -1,34 +1,27 @@ -#!/usr/bin/env python3 - import os import sys import signal -import tf import math import rospy import rospkg -import yaml from qt_gui.plugin import Plugin from python_qt_binding import loadUi from python_qt_binding.QtWidgets import QWidget, QFileDialog, QMessageBox -from grasplan.rqt_grasplan.grasps import Grasps -from grasplan.visualization.grasp_visualizer import GraspVisualizer - -from std_msgs.msg import Int8, String -from geometry_msgs.msg import Pose, PoseArray, PoseStamped from tf.transformations import quaternion_from_euler from grasplan.rqt_planning_scene.visualize_planning_scene import PlanningSceneVizSettings, PlanningSceneViz from grasplan.rqt_planning_scene.rosbag_interval_pub import RosbagIntervalPub + class OpenFileDialog(QWidget): ''' allow the user to select a different yaml file with a button, this will open a dialog to select and open a yaml file ''' + def __init__(self, initial_path=None): super().__init__() left, top, width, height = 10, 10, 640, 480 @@ -43,19 +36,21 @@ def openFileNameDialog(self, save_file_name_dialog=False): else: initial_path = self.initial_path if save_file_name_dialog: - fileName, _ = QFileDialog.getSaveFileName(self, 'Select boxes yaml file',\ - initial_path, 'Yaml Files (*.yaml)', options=options) + fileName, _ = QFileDialog.getSaveFileName( + self, 'Select boxes yaml file', initial_path, 'Yaml Files (*.yaml)', options=options + ) else: - fileName, _ = QFileDialog.getOpenFileName(self, 'Select boxes yaml file',\ - initial_path, 'Yaml Files (*.yaml)', options=options) + fileName, _ = QFileDialog.getOpenFileName( + self, 'Select boxes yaml file', initial_path, 'Yaml Files (*.yaml)', options=options + ) if fileName: return fileName def saveFileNameDialog(self): return self.openFileNameDialog(save_file_name_dialog=True) -class RqtPlanningScene(Plugin): +class RqtPlanningScene(Plugin): def __init__(self, context): super(RqtPlanningScene, self).__init__(context) rospy.loginfo('Initializing grasplan rqt, have a happy planning scene editing !') @@ -97,9 +92,13 @@ def __init__(self, context): # parameters settings = PlanningSceneVizSettings() - settings.yaml_path_to_read = rospy.get_param('~yaml_path_to_read', grasplan_path + '/config/examples/planning_scene.yaml') - settings.yaml_path_to_write = rospy.get_param('~yaml_path_to_write', grasplan_path + '/config/examples/auto_generated_planning_scene.yaml') - self.psv = PlanningSceneViz(settings) # TODO: add use case: create planning scene from scratch + settings.yaml_path_to_read = rospy.get_param( + '~yaml_path_to_read', grasplan_path + '/config/examples/planning_scene.yaml' + ) + settings.yaml_path_to_write = rospy.get_param( + '~yaml_path_to_write', grasplan_path + '/config/examples/auto_generated_planning_scene.yaml' + ) + self.psv = PlanningSceneViz(settings) # TODO: add use case: create planning scene from scratch self.psv.publish_boxes() # publications @@ -129,30 +128,34 @@ def __init__(self, context): self._widget.comboExistingBoxes.currentIndexChanged.connect(self.comboExistingBoxes_changed) # chk hide changes - self.hide_chks = [self._widget.chkHide1, - self._widget.chkHide2, - self._widget.chkHide3, - self._widget.chkHide4, - self._widget.chkHide5, - self._widget.chkHide6, - self._widget.chkHide7, - self._widget.chkHide8, - self._widget.chkHide9, - self._widget.chkHide10] + self.hide_chks = [ + self._widget.chkHide1, + self._widget.chkHide2, + self._widget.chkHide3, + self._widget.chkHide4, + self._widget.chkHide5, + self._widget.chkHide6, + self._widget.chkHide7, + self._widget.chkHide8, + self._widget.chkHide9, + self._widget.chkHide10, + ] for hide_chk in self.hide_chks: hide_chk.stateChanged.connect(self.chkHide_changed) # viz widgets - self.viz_widgets = [self._widget.txtViz1, - self._widget.txtViz2, - self._widget.txtViz3, - self._widget.txtViz4, - self._widget.txtViz5, - self._widget.txtViz6, - self._widget.txtViz7, - self._widget.txtViz8, - self._widget.txtViz9, - self._widget.txtViz10] + self.viz_widgets = [ + self._widget.txtViz1, + self._widget.txtViz2, + self._widget.txtViz3, + self._widget.txtViz4, + self._widget.txtViz5, + self._widget.txtViz6, + self._widget.txtViz7, + self._widget.txtViz8, + self._widget.txtViz9, + self._widget.txtViz10, + ] self.populate_visibility_panel_box_names() @@ -183,7 +186,7 @@ def populate_visibility_panel_box_names(self): def slideRosbagProgress_slider_released(self): self.lock_progress_bar = False rosbag_progress_slide_value = self._widget.slideRosbagProgress.value() - percentage_to_play = 1.0 # parameter to control how much from the bag to play in percentage + percentage_to_play = 1.0 # parameter to control how much from the bag to play in percentage delta = percentage_to_play / 2.0 start_value = rosbag_progress_slide_value - delta if rosbag_progress_slide_value - delta < 0.0: @@ -226,26 +229,26 @@ def handle_cmdReset(self): def update_slide_values(self, scene_name): box = self.psv.get_box_values(scene_name) - value_x, min_x, max_x = self.get_slide_value_and_limits('x') - value_y, min_y, max_y = self.get_slide_value_and_limits('y') - value_z, min_z, max_z = self.get_slide_value_and_limits('z') + value_x, min_x, max_x = self.get_slide_value_and_limits('x') + value_y, min_y, max_y = self.get_slide_value_and_limits('y') + value_z, min_z, max_z = self.get_slide_value_and_limits('z') box_position_x = box['box_position_x'] box_position_y = box['box_position_y'] box_position_z = box['box_position_z'] # make sure box values are within range - #continue_ = False - #if box_position_x > min_x: - #if box_position_x < max_x: - #if box_position_y < min_y: - #if box_position_y < max_y: - #if box_position_z < min_z: - #if box_position_z < max_z: - #continue_ = True - #if not continue_: - #rospy.logwarn('box values are outside limits, will not update slide values') - #return + # continue_ = False + # if box_position_x > min_x: + # if box_position_x < max_x: + # if box_position_y < min_y: + # if box_position_y < max_y: + # if box_position_z < min_z: + # if box_position_z < max_z: + # continue_ = True + # if not continue_: + # rospy.logwarn('box values are outside limits, will not update slide values') + # return # diff -> 100% # diff = max_scroll_limit - min_scroll_limit # value -> slide_value? # value = box_position_x @@ -303,7 +306,7 @@ def handle_cmdSaveYaml(self): ofd = OpenFileDialog(initial_path=grasplan_path) yaml_path = ofd.saveFileNameDialog() if yaml_path: - if not '.yaml' in yaml_path: + if '.yaml' not in yaml_path: yaml_path += '.yaml' self.psv.settings.yaml_path_to_write = yaml_path self.psv.write_boxes_to_yaml(yaml_path) @@ -313,11 +316,17 @@ def handle_cmdSaveYaml(self): def update_angles(self): quaternion = quaternion_from_euler(self.roll_angle, self.pitch_angle, self.yaw_angle) - self.psv.modify_box(self.selected_box, - modify_box_orientation_x=True, box_orientation_x=float(quaternion[0]), - modify_box_orientation_y=True, box_orientation_y=float(quaternion[1]), - modify_box_orientation_z=True, box_orientation_z=float(quaternion[2]), - modify_box_orientation_w=True, box_orientation_w=float(quaternion[3])) + self.psv.modify_box( + self.selected_box, + modify_box_orientation_x=True, + box_orientation_x=float(quaternion[0]), + modify_box_orientation_y=True, + box_orientation_y=float(quaternion[1]), + modify_box_orientation_z=True, + box_orientation_z=float(quaternion[2]), + modify_box_orientation_w=True, + box_orientation_w=float(quaternion[3]), + ) # write values to rqt self._widget.txtTFQx.setPlainText(str(round(quaternion[0], 6))) self._widget.txtTFQy.setPlainText(str(round(quaternion[1], 6))) @@ -343,7 +352,7 @@ def slideRoll_value_changed(self): self.handle_angle_change(self._widget.slideRoll.value(), 'roll') def slidePitch_value_changed(self): - self.handle_angle_change(self._widget.slidePitch.value(), 'pitch') + self.handle_angle_change(self._widget.slidePitch.value(), 'pitch') def slideYaw_value_changed(self): self.handle_angle_change(self._widget.slideYaw.value(), 'yaw') @@ -369,17 +378,23 @@ def get_slide_value_and_limits(self, slide): slide can take values of 'x', 'y', or 'z' ''' if slide == 'x': - return float(self._widget.slideX.value()),\ - float(self._widget.txtScrollMinX.toPlainText()),\ - float(self._widget.txtScrollMaxX.toPlainText()) + return ( + float(self._widget.slideX.value()), + float(self._widget.txtScrollMinX.toPlainText()), + float(self._widget.txtScrollMaxX.toPlainText()), + ) elif slide == 'y': - return float(self._widget.slideY.value()),\ - float(self._widget.txtScrollMinY.toPlainText()),\ - float(self._widget.txtScrollMaxY.toPlainText()) + return ( + float(self._widget.slideY.value()), + float(self._widget.txtScrollMinY.toPlainText()), + float(self._widget.txtScrollMaxY.toPlainText()), + ) elif slide == 'z': - return float(self._widget.slideZ.value()),\ - float(self._widget.txtScrollMinZ.toPlainText()),\ - float(self._widget.txtScrollMaxZ.toPlainText()) + return ( + float(self._widget.slideZ.value()), + float(self._widget.txtScrollMinZ.toPlainText()), + float(self._widget.txtScrollMaxZ.toPlainText()), + ) def slideX_value_changed(self): value = self.compute_value('x') diff --git a/src/grasplan/rqt_planning_scene/visualize_planning_scene.py b/src/grasplan/rqt_planning_scene/visualize_planning_scene.py index 445e5ab..3b4a698 100755 --- a/src/grasplan/rqt_planning_scene/visualize_planning_scene.py +++ b/src/grasplan/rqt_planning_scene/visualize_planning_scene.py @@ -16,6 +16,7 @@ from visualization_msgs.msg import Marker, MarkerArray from geometry_msgs.msg import PoseStamped + class PlanningSceneVizSettings: # default settings text_on = True @@ -29,6 +30,7 @@ class PlanningSceneVizSettings: yaml_path_to_write = '' publish_tf = True + class PlanningSceneViz: def __init__(self, settings, load_boxes_from_yaml=True): self.settings = settings @@ -77,30 +79,58 @@ def reset_scene_name(self, scene_name): if box['scene_name'] == scene_name: for box_bkp in self.box_list_dictionary_bkp: if box_bkp['scene_name'] == scene_name: - self.modify_box(scene_name, modify_box_position_x=True, box_position_x=box_bkp['box_position_x'], modify_box_position_y=True, box_position_y=box_bkp['box_position_y'], - modify_box_position_z=True, box_position_z=box_bkp['box_position_z'], - modify_box_orientation_x=True, box_orientation_x=box_bkp['box_orientation_x'], - modify_box_orientation_y=True, box_orientation_y=box_bkp['box_orientation_y'], - modify_box_orientation_z=True, box_orientation_z=box_bkp['box_orientation_z'], - modify_box_orientation_w=True, box_orientation_w=box_bkp['box_orientation_w'], - modify_box_x_dimension=True, box_x_dimension=box_bkp['box_x_dimension'], - modify_box_y_dimension=True, box_y_dimension=box_bkp['box_y_dimension'], - modify_box_z_dimension=True, box_z_dimension=box_bkp['box_z_dimension']) + self.modify_box( + scene_name, + modify_box_position_x=True, + box_position_x=box_bkp['box_position_x'], + modify_box_position_y=True, + box_position_y=box_bkp['box_position_y'], + modify_box_position_z=True, + box_position_z=box_bkp['box_position_z'], + modify_box_orientation_x=True, + box_orientation_x=box_bkp['box_orientation_x'], + modify_box_orientation_y=True, + box_orientation_y=box_bkp['box_orientation_y'], + modify_box_orientation_z=True, + box_orientation_z=box_bkp['box_orientation_z'], + modify_box_orientation_w=True, + box_orientation_w=box_bkp['box_orientation_w'], + modify_box_x_dimension=True, + box_x_dimension=box_bkp['box_x_dimension'], + modify_box_y_dimension=True, + box_y_dimension=box_bkp['box_y_dimension'], + modify_box_z_dimension=True, + box_z_dimension=box_bkp['box_z_dimension'], + ) return rospy.logerr(f'could not found scene name: {scene_name}') - def modify_box(self, scene_name, - modify_box_position_x=False, box_position_x=0.0, - modify_box_position_y=False, box_position_y=0.0, - modify_box_position_z=False, box_position_z=0.0, - modify_box_orientation_x=False, box_orientation_x=0.0, - modify_box_orientation_y=False, box_orientation_y=0.0, - modify_box_orientation_z=False, box_orientation_z=0.0, - modify_box_orientation_w=False, box_orientation_w=1.0, - modify_box_x_dimension=False, box_x_dimension=1.0, - modify_box_y_dimension=False, box_y_dimension=1.0, - modify_box_z_dimension=False, box_z_dimension=1.0, - modify_frame_id=False, new_frame_id='world'): + def modify_box( + self, + scene_name, + modify_box_position_x=False, + box_position_x=0.0, + modify_box_position_y=False, + box_position_y=0.0, + modify_box_position_z=False, + box_position_z=0.0, + modify_box_orientation_x=False, + box_orientation_x=0.0, + modify_box_orientation_y=False, + box_orientation_y=0.0, + modify_box_orientation_z=False, + box_orientation_z=0.0, + modify_box_orientation_w=False, + box_orientation_w=1.0, + modify_box_x_dimension=False, + box_x_dimension=1.0, + modify_box_y_dimension=False, + box_y_dimension=1.0, + modify_box_z_dimension=False, + box_z_dimension=1.0, + modify_frame_id=False, + new_frame_id='world', + ): found = False for box in self.box_list_dictionary: if box['scene_name'] == scene_name: @@ -133,7 +163,7 @@ def modify_box(self, scene_name, def load_boxes_from_yaml(self, yaml_path): if yaml_path == '': - rospy.logwarn(f'empty yaml path') + rospy.logwarn('empty yaml path') return if not os.path.exists(yaml_path): rospy.logerr(f'yaml path does not exist: {yaml_path}') @@ -173,7 +203,9 @@ def transform_to_frame(self, object_frame, target_frame): input_pose.pose.orientation.z = box['box_orientation_z'] input_pose.pose.orientation.w = box['box_orientation_w'] # Wait for the transform to be available - self.listener.waitForTransform(target_frame, input_pose.header.frame_id, rospy.Time(), rospy.Duration(1.0)) + self.listener.waitForTransform( + target_frame, input_pose.header.frame_id, rospy.Time(), rospy.Duration(1.0) + ) transformed_pose = self.listener.transformPose(target_frame, input_pose) return transformed_pose @@ -189,24 +221,34 @@ def change_boxes_ref_frame(self, target_frame_dic): rospy.loginfo(f'expressing {key} in {target_frame_dic[key]} ref frame') # perform the transformation new_pose = self.transform_to_frame(key, target_frame_dic[key]) - x = new_pose.pose.position.x - y = new_pose.pose.position.y - z = new_pose.pose.position.z - qx = new_pose.pose.orientation.x - qy = new_pose.pose.orientation.y - qz = new_pose.pose.orientation.z - qw = new_pose.pose.orientation.w + # x = new_pose.pose.position.x + # y = new_pose.pose.position.y + # z = new_pose.pose.position.z + # qx = new_pose.pose.orientation.x + # qy = new_pose.pose.orientation.y + # qz = new_pose.pose.orientation.z + # qw = new_pose.pose.orientation.w # next line is useful for debugging purposes # self.broadcast_tf(x, y, z, qx, qy, qz, qw, target_frame_dic[key], f'{key}_debug') - self.modify_box(box['scene_name'], - modify_box_position_x=True, box_position_x=round(float(new_pose.pose.position.x), 3), - modify_box_position_y=True, box_position_y=round(float(new_pose.pose.position.y), 3), - modify_box_position_z=True, box_position_z=round(float(new_pose.pose.position.z), 3), - modify_box_orientation_x=True, box_orientation_x=round(float(new_pose.pose.orientation.x), 4), - modify_box_orientation_y=True, box_orientation_y=round(float(new_pose.pose.orientation.y), 4), - modify_box_orientation_z=True, box_orientation_z=round(float(new_pose.pose.orientation.z), 4), - modify_box_orientation_w=True, box_orientation_w=round(float(new_pose.pose.orientation.w), 4), - modify_frame_id=True, new_frame_id=new_pose.header.frame_id) + self.modify_box( + box['scene_name'], + modify_box_position_x=True, + box_position_x=round(float(new_pose.pose.position.x), 3), + modify_box_position_y=True, + box_position_y=round(float(new_pose.pose.position.y), 3), + modify_box_position_z=True, + box_position_z=round(float(new_pose.pose.position.z), 3), + modify_box_orientation_x=True, + box_orientation_x=round(float(new_pose.pose.orientation.x), 4), + modify_box_orientation_y=True, + box_orientation_y=round(float(new_pose.pose.orientation.y), 4), + modify_box_orientation_z=True, + box_orientation_z=round(float(new_pose.pose.orientation.z), 4), + modify_box_orientation_w=True, + box_orientation_w=round(float(new_pose.pose.orientation.w), 4), + modify_frame_id=True, + new_frame_id=new_pose.header.frame_id, + ) def symbolic_to_rgb_color(self, symbolic_color): if symbolic_color == 'green': @@ -220,7 +262,7 @@ def symbolic_to_rgb_color(self, symbolic_color): if symbolic_color == 'orange': return [1.0, 0.65, 0.0] rospy.logwarn(f'color {symbolic_color} not supported, will use default green color') - rospy.logwarn(f'supported colors are: green, blue, red, purple, orange') + rospy.logwarn('supported colors are: green, blue, red, purple, orange') return [0.0, 1.0, 0.0] def make_text_marker(self, marker, text): @@ -232,16 +274,28 @@ def make_text_marker(self, marker, text): self.marker_id_count += 1 marker.text = text marker.pose.position.z += marker.scale.z / 2.0 + 0.1 - marker.scale.z = 0.2 # text size + marker.scale.z = 0.2 # text size marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 return marker - def make_marker(self, scene_name, frame_id, origin_x, origin_y, origin_z, - orientation_x, orientation_y, orientation_z, orientation_w, - scale_x, scale_y, scale_z): - rgb_color_list = self.symbolic_to_rgb_color('green') # default + def make_marker( + self, + scene_name, + frame_id, + origin_x, + origin_y, + origin_z, + orientation_x, + orientation_y, + orientation_z, + orientation_w, + scale_x, + scale_y, + scale_z, + ): + rgb_color_list = self.symbolic_to_rgb_color('green') # default if scene_name in self.settings.colors: rgb_color_list = self.symbolic_to_rgb_color(self.settings.colors[scene_name]) marker = Marker() @@ -294,7 +348,7 @@ def wait_for_subscribers(self): def broadcast_tf(self, x, y, z, qx, qy, qz, qw, parent_frame_id, child_frame_id): if self.br: - self.br.sendTransform((x,y,z), (qx, qy, qz, qw), rospy.Time.now(), child_frame_id, parent_frame_id) + self.br.sendTransform((x, y, z), (qx, qy, qz, qw), rospy.Time.now(), child_frame_id, parent_frame_id) else: rospy.logerr('cannot broadcast to tf') @@ -304,9 +358,17 @@ def publish_tf(self): does not take into account ignore_set ''' for box in self.box_list_dictionary: - self.broadcast_tf(box['box_position_x'], box['box_position_y'], box['box_position_z'],\ - box['box_orientation_x'], box['box_orientation_y'], box['box_orientation_z'], box['box_orientation_w'],\ - box['frame_id'], box['scene_name']) + self.broadcast_tf( + box['box_position_x'], + box['box_position_y'], + box['box_position_z'], + box['box_orientation_x'], + box['box_orientation_y'], + box['box_orientation_z'], + box['box_orientation_w'], + box['frame_id'], + box['scene_name'], + ) def publish_boxes(self): self.delete_all_markers() @@ -321,29 +383,40 @@ def publish_boxes(self): for box in self.box_list_dictionary: if box['scene_name'] in ignore_set: continue - marker = self.make_marker(box['scene_name'], - box['frame_id'], - box['box_position_x'], - box['box_position_y'], - box['box_position_z'], - box['box_orientation_x'], - box['box_orientation_y'], - box['box_orientation_z'], - box['box_orientation_w'], - box['box_x_dimension'], - box['box_y_dimension'], - box['box_z_dimension']) + marker = self.make_marker( + box['scene_name'], + box['frame_id'], + box['box_position_x'], + box['box_position_y'], + box['box_position_z'], + box['box_orientation_x'], + box['box_orientation_y'], + box['box_orientation_z'], + box['box_orientation_w'], + box['box_x_dimension'], + box['box_y_dimension'], + box['box_z_dimension'], + ) marker_array_msg.markers.append(marker) if self.settings.text_on: text_marker = self.make_text_marker(copy.deepcopy(marker), box['scene_name']) marker_array_msg.markers.append(text_marker) if self.settings.publish_tf: - self.broadcast_tf(box['box_position_x'], box['box_position_y'], box['box_position_z'],\ - box['box_orientation_x'], box['box_orientation_y'], box['box_orientation_z'], box['box_orientation_w'],\ - box['frame_id'], box['scene_name']) + self.broadcast_tf( + box['box_position_x'], + box['box_position_y'], + box['box_position_z'], + box['box_orientation_x'], + box['box_orientation_y'], + box['box_orientation_z'], + box['box_orientation_w'], + box['frame_id'], + box['scene_name'], + ) self.marker_array_pub.publish(marker_array_msg) + if __name__ == '__main__': rospy.init_node('planning_scene_publisher', anonymous=False) if len(sys.argv) > 1: diff --git a/src/grasplan/snippets/goto_cartesian_pose.py b/src/grasplan/snippets/goto_cartesian_pose.py index f086a87..e28a7ab 100755 --- a/src/grasplan/snippets/goto_cartesian_pose.py +++ b/src/grasplan/snippets/goto_cartesian_pose.py @@ -6,6 +6,7 @@ arm = moveit_commander.MoveGroupCommander('arm', wait_for_servers=10.0) + def test_go_to_cartesian_pose(): ''' goto cartesian pose with moveit @@ -28,20 +29,21 @@ def test_go_to_cartesian_pose(): # rotation = [-0.703003, 0.082303, 0.701726, 0.081197] rospy.loginfo(f'planning frame : {arm.get_planning_frame()}') - target_pose.header.frame_id = arm.get_planning_frame() # 'world' - target_pose.pose.position.x = translation[0] - target_pose.pose.position.y = translation[1] - target_pose.pose.position.z = translation[2] - target_pose.pose.orientation.x = rotation[0] - target_pose.pose.orientation.y = rotation[1] - target_pose.pose.orientation.z = rotation[2] - target_pose.pose.orientation.w = rotation[3] + target_pose.header.frame_id = arm.get_planning_frame() # 'world' + target_pose.pose.position.x = translation[0] + target_pose.pose.position.y = translation[1] + target_pose.pose.position.z = translation[2] + target_pose.pose.orientation.x = rotation[0] + target_pose.pose.orientation.y = rotation[1] + target_pose.pose.orientation.z = rotation[2] + target_pose.pose.orientation.w = rotation[3] rospy.loginfo('going to cartesian pose') arm.set_pose_target(target_pose, end_effector_link='hand_ee_link') arm.go() rospy.loginfo('finished going to cartesian pose...') rospy.spin() -if __name__=='__main__': + +if __name__ == '__main__': rospy.init_node('goto_cart_test_node', anonymous=False) test_go_to_cartesian_pose() diff --git a/src/grasplan/test/common_grasp_tools_tests.py b/src/grasplan/test/common_grasp_tools_tests.py old mode 100644 new mode 100755 index 765efbe..980a617 --- a/src/grasplan/test/common_grasp_tools_tests.py +++ b/src/grasplan/test/common_grasp_tools_tests.py @@ -3,8 +3,8 @@ import unittest from grasplan.common_grasp_tools import separate_object_class_from_id -class TestCommonGraspTools(unittest.TestCase): +class TestCommonGraspTools(unittest.TestCase): def test_normal(self): self.assertEquals(separate_object_class_from_id('relay_1'), ('relay', 1)) @@ -53,6 +53,8 @@ def test_multimeter(self): def test_only_class_required(self): self.assertEquals(separate_object_class_from_id('multimeter')[0], 'multimeter') + if __name__ == '__main__': import rostest + rostest.rosrun('grasplan', 'test_cgt', TestCommonGraspTools) diff --git a/src/grasplan/tools/common.py b/src/grasplan/tools/common.py index 90408bd..74fbe67 100755 --- a/src/grasplan/tools/common.py +++ b/src/grasplan/tools/common.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 + def separate_object_class_from_id(anchored_object_as_string): ''' e.g. input : relay_1 @@ -14,12 +15,13 @@ def separate_object_class_from_id(anchored_object_as_string): break chars_before_number = anchored_object_as_string[:-count] try: - object_id = int(anchored_object_as_string[-count+1:]) + object_id = int(anchored_object_as_string[-count + 1 :]) return chars_before_number, object_id - except: + except ValueError: return anchored_object_as_string, None -class objectToPick(): + +class objectToPick: def __init__(self, obj_class_and_id_as_string=None): ''' this class is meant to be used as a struct that holds info on: @@ -29,7 +31,7 @@ def __init__(self, obj_class_and_id_as_string=None): self.obj_class = None self.id = None self.any_obj_id = None - if not obj_class_and_id_as_string is None: + if obj_class_and_id_as_string is not None: assert isinstance(obj_class_and_id_as_string, str) self.obj_class, self.id = separate_object_class_from_id(obj_class_and_id_as_string) if self.id is None: diff --git a/src/grasplan/tools/moveit_errors.py b/src/grasplan/tools/moveit_errors.py index b364c6a..fb99919 100644 --- a/src/grasplan/tools/moveit_errors.py +++ b/src/grasplan/tools/moveit_errors.py @@ -1,8 +1,7 @@ -#!/usr/bin/env python3 - import rospy from moveit_msgs.msg import MoveItErrorCodes + def print_moveit_error_helper(error_code, moveit_error_code, moveit_error_string): ''' function that helps print_moveit_error method to have less code @@ -10,24 +9,32 @@ def print_moveit_error_helper(error_code, moveit_error_code, moveit_error_string if error_code == moveit_error_code: rospy.logwarn(f'moveit says : {moveit_error_string}') + def print_moveit_error(error_code): ''' receive moveit result, compare with error codes, print what happened ''' print_moveit_error_helper(error_code, MoveItErrorCodes.PLANNING_FAILED, 'PLANNING_FAILED') print_moveit_error_helper(error_code, MoveItErrorCodes.INVALID_MOTION_PLAN, 'INVALID_MOTION_PLAN') - print_moveit_error_helper(error_code, MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE,\ - 'MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE') + print_moveit_error_helper( + error_code, + MoveItErrorCodes.MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE, + 'MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE', + ) print_moveit_error_helper(error_code, MoveItErrorCodes.CONTROL_FAILED, 'CONTROL_FAILED') print_moveit_error_helper(error_code, MoveItErrorCodes.UNABLE_TO_AQUIRE_SENSOR_DATA, 'UNABLE_TO_AQUIRE_SENSOR_DATA') print_moveit_error_helper(error_code, MoveItErrorCodes.TIMED_OUT, 'TIMED_OUT') print_moveit_error_helper(error_code, MoveItErrorCodes.PREEMPTED, 'PREEMPTED') print_moveit_error_helper(error_code, MoveItErrorCodes.START_STATE_IN_COLLISION, 'START_STATE_IN_COLLISION') - print_moveit_error_helper(error_code, MoveItErrorCodes.START_STATE_VIOLATES_PATH_CONSTRAINTS, 'START_STATE_VIOLATES_PATH_CONSTRAINTS') + print_moveit_error_helper( + error_code, MoveItErrorCodes.START_STATE_VIOLATES_PATH_CONSTRAINTS, 'START_STATE_VIOLATES_PATH_CONSTRAINTS' + ) print_moveit_error_helper(error_code, MoveItErrorCodes.GOAL_IN_COLLISION, 'GOAL_IN_COLLISION') - print_moveit_error_helper(error_code, MoveItErrorCodes.GOAL_VIOLATES_PATH_CONSTRAINTS, 'GOAL_VIOLATES_PATH_CONSTRAINTS') + print_moveit_error_helper( + error_code, MoveItErrorCodes.GOAL_VIOLATES_PATH_CONSTRAINTS, 'GOAL_VIOLATES_PATH_CONSTRAINTS' + ) print_moveit_error_helper(error_code, MoveItErrorCodes.GOAL_CONSTRAINTS_VIOLATED, 'GOAL_CONSTRAINTS_VIOLATED') print_moveit_error_helper(error_code, MoveItErrorCodes.INVALID_GROUP_NAME, 'INVALID_GROUP_NAME') @@ -37,7 +44,9 @@ def print_moveit_error(error_code): print_moveit_error_helper(error_code, MoveItErrorCodes.INVALID_OBJECT_NAME, 'INVALID_OBJECT_NAME') print_moveit_error_helper(error_code, MoveItErrorCodes.FRAME_TRANSFORM_FAILURE, 'FRAME_TRANSFORM_FAILURE') - print_moveit_error_helper(error_code, MoveItErrorCodes.COLLISION_CHECKING_UNAVAILABLE, 'COLLISION_CHECKING_UNAVAILABLE') + print_moveit_error_helper( + error_code, MoveItErrorCodes.COLLISION_CHECKING_UNAVAILABLE, 'COLLISION_CHECKING_UNAVAILABLE' + ) print_moveit_error_helper(error_code, MoveItErrorCodes.ROBOT_STATE_STALE, 'ROBOT_STATE_STALE') print_moveit_error_helper(error_code, MoveItErrorCodes.SENSOR_INFO_STALE, 'SENSOR_INFO_STALE') print_moveit_error_helper(error_code, MoveItErrorCodes.COMMUNICATION_FAILURE, 'COMMUNICATION_FAILURE') diff --git a/src/grasplan/tools/support_plane_tools.py b/src/grasplan/tools/support_plane_tools.py index 2edbeb4..b91f33a 100755 --- a/src/grasplan/tools/support_plane_tools.py +++ b/src/grasplan/tools/support_plane_tools.py @@ -41,22 +41,29 @@ def make_plane_marker_msg(ref_frame, plane): marker_msg.points.append(plane[3]) marker_msg.points.append(plane[0]) marker_msg.scale = Vector3(1.0, 1.0, 1.0) - marker_msg.color = ColorRGBA(1.0, 0.61, 0.16, 1.0) # orange + marker_msg.color = ColorRGBA(1.0, 0.61, 0.16, 1.0) # orange return marker_msg + def compute_object_height_for_insertion(object_class_tbi, support_obj_class, gap_between_objects=0.02): # ohd : objects height dictionary, object_class_tbi : object class to be inserted - ohd = {'power_drill_with_grip': 0.2205359935760498, - 'klt': 0.14699999809265138, - 'multimeter': 0.04206399992108345, - 'relay': 0.10436400026082993, - 'screwdriver': 0.034412000328302383} + ohd = { + 'power_drill_with_grip': 0.2205359935760498, + 'klt': 0.14699999809265138, + 'multimeter': 0.04206399992108345, + 'relay': 0.10436400026082993, + 'screwdriver': 0.034412000328302383, + } return (ohd[support_obj_class] / 2.0) + (ohd[object_class_tbi] / 2.0) + gap_between_objects -def gen_insert_poses_from_obj(object_class, support_object_pose, obj_height, frame_id='map', same_orientation_as_support_obj=False): + +def gen_insert_poses_from_obj( + object_class, support_object_pose, obj_height, frame_id='map', same_orientation_as_support_obj=False +): ''' if same_orientation_as_support_obj is True then the object is aligned with the support object (e.g. box) - if same_orientation_as_support_obj is False then 360 degree orientations are used to place the object (can be used if first time fails) + if same_orientation_as_support_obj is False then 360 degree orientations are used to place the object (can + be used if first time fails) ''' object_list_msg = ObjectList() object_list_msg.header.frame_id = frame_id @@ -72,8 +79,8 @@ def gen_insert_poses_from_obj(object_class, support_object_pose, obj_height, fra yaw = 0.0 # HACK: object specific rotations if object_class == 'power_drill_with_grip': - roll = - math.pi / 2.0 - + roll = -math.pi / 2.0 + if not same_orientation_as_support_obj: for i in range(7): angular_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) @@ -83,8 +90,8 @@ def gen_insert_poses_from_obj(object_class, support_object_pose, obj_height, fra object_pose_msg.pose.orientation.w = angular_q[3] object_pose_msg.instance_id = insert_poses_id object_list_msg.objects.append(copy.deepcopy(object_pose_msg)) - insert_poses_id +=1 - yaw += 0.5 # ~ 30 degree + insert_poses_id += 1 + yaw += 0.5 # ~ 30 degree else: object_pose_msg.pose.orientation.x = support_object_pose.pose.orientation.x object_pose_msg.pose.orientation.y = support_object_pose.pose.orientation.y @@ -92,20 +99,27 @@ def gen_insert_poses_from_obj(object_class, support_object_pose, obj_height, fra object_pose_msg.pose.orientation.w = support_object_pose.pose.orientation.w object_pose_msg.instance_id = insert_poses_id object_list_msg.objects.append(copy.deepcopy(object_pose_msg)) - insert_poses_id +=1 - q = [support_object_pose.pose.orientation.x, support_object_pose.pose.orientation.y, - support_object_pose.pose.orientation.z, support_object_pose.pose.orientation.w] + insert_poses_id += 1 + q = [ + support_object_pose.pose.orientation.x, + support_object_pose.pose.orientation.y, + support_object_pose.pose.orientation.z, + support_object_pose.pose.orientation.w, + ] euler_rot = tf.transformations.euler_from_quaternion(q) - q_new = tf.transformations.quaternion_from_euler(euler_rot[0], euler_rot[1], euler_rot[2] + 3.14159) # yaw + 180 degree + q_new = tf.transformations.quaternion_from_euler( + euler_rot[0], euler_rot[1], euler_rot[2] + 3.14159 + ) # yaw + 180 degree object_pose_msg.pose.orientation.x = q_new[0] object_pose_msg.pose.orientation.y = q_new[1] object_pose_msg.pose.orientation.z = q_new[2] object_pose_msg.pose.orientation.w = q_new[3] object_pose_msg.instance_id = insert_poses_id object_list_msg.objects.append(copy.deepcopy(object_pose_msg)) - insert_poses_id +=1 + insert_poses_id += 1 return object_list_msg + def well_separated(x_y_list, candidate_x, candidate_y, min_dist=0.2): if len(x_y_list) == 0: return True @@ -117,14 +131,25 @@ def well_separated(x_y_list, candidate_x, candidate_y, min_dist=0.2): return True return False -def gen_place_poses_from_plane(object_class: str, support_object:str, plane: List[str], planning_scene: PlanningScene, frame_id:str = "map", - number_of_poses: int = 10, min_dist: float = 0.2, ignore_min_dist_list: List[str] = []): + +def gen_place_poses_from_plane( + object_class: str, + support_object: str, + plane: List[str], + planning_scene: PlanningScene, + frame_id: str = "map", + number_of_poses: int = 10, + min_dist: float = 0.2, + ignore_min_dist_list: List[str] = [], +): ''' random sample poses within a plane and populate object list msg with the result ''' if number_of_poses > 100: min_dist = 0.03 - rospy.logwarn(f'number of poses is greater than 100, min_dist will be set to {0.03} instead of desired value of {min_dist}') + rospy.logwarn( + f'number of poses is greater than 100, min_dist will be set to 0.03 instead of desired value of {min_dist}' + ) object_list_msg = ObjectList() object_list_msg.header.frame_id = frame_id x_y_list = [] @@ -142,7 +167,7 @@ def gen_place_poses_from_plane(object_class: str, support_object:str, plane: Lis if well_separated(x_y_list, candidate_x, candidate_y, min_dist=min_dist): break count += 1 - if count > 50000: # avoid an infinite loop, cap the max attempts + if count > 50000: # avoid an infinite loop, cap the max attempts rospy.logwarn(f'Could not generate poses too much separated from each other, min dist : {min_dist}') break x_y_list.append([candidate_x, candidate_y]) @@ -157,7 +182,7 @@ def gen_place_poses_from_plane(object_class: str, support_object:str, plane: Lis yaw = round(random.uniform(0.0, math.pi), 4) # HACK: object specific rotations if object_class == 'power_drill_with_grip': - roll = - math.pi / 2.0 + roll = -math.pi / 2.0 if number_of_poses > 20: rospy.loginfo('covering 360 angle for each pose') yaw = 0.0 @@ -169,8 +194,8 @@ def gen_place_poses_from_plane(object_class: str, support_object:str, plane: Lis object_pose_msg.pose.orientation.w = angular_q[3] object_pose_msg.instance_id = place_poses_id object_list_msg.objects.append(copy.deepcopy(object_pose_msg)) - place_poses_id +=1 - yaw += 0.5 # ~ 30 degree + place_poses_id += 1 + yaw += 0.5 # ~ 30 degree else: angular_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) object_pose_msg.pose.orientation.x = angular_q[0] @@ -179,17 +204,14 @@ def gen_place_poses_from_plane(object_class: str, support_object:str, plane: Lis object_pose_msg.pose.orientation.w = angular_q[3] object_pose_msg.instance_id = place_poses_id object_list_msg.objects.append(copy.deepcopy(object_pose_msg)) - place_poses_id +=1 + place_poses_id += 1 return object_list_msg + # TODO: consider using shape_msgs/Plane instead of 4 points def adjust_plane( - plane: List[Point], - x_extend: float = 0.0, - y_extend: float = 0.0, - x_offset: float = 0.0, - y_offset: float = 0.0 - ) -> List[Point]: + plane: List[Point], x_extend: float = 0.0, y_extend: float = 0.0, x_offset: float = 0.0, y_offset: float = 0.0 +) -> List[Point]: """ Adjust the dimensions and position of a plane. @@ -224,10 +246,13 @@ def adjust_plane( min_x, max_x = min(p.x for p in plane), max(p.x for p in plane) min_y, max_y = min(p.y for p in plane), max(p.y for p in plane) - return [Point(min_x - x_extend, min_y - y_extend, plane[0].z), - Point(max_x + x_extend, min_y - y_extend, plane[0].z), - Point(max_x + x_extend, max_y + y_extend, plane[0].z), - Point(min_x - x_extend, max_y + y_extend, plane[0].z)] + return [ + Point(min_x - x_extend, min_y - y_extend, plane[0].z), + Point(max_x + x_extend, min_y - y_extend, plane[0].z), + Point(max_x + x_extend, max_y + y_extend, plane[0].z), + Point(min_x - x_extend, max_y + y_extend, plane[0].z), + ] + def visualize_points(points: List[Point], point_publisher: rospy.Publisher) -> None: """Publish points to view them in RViz""" @@ -235,6 +260,7 @@ def visualize_points(points: List[Point], point_publisher: rospy.Publisher) -> N point_publisher.publish(PointStamped(header=Header(frame_id='map'), point=point)) rospy.sleep(0.5) + def get_obj_from_planning_scene(obj_name: str, planning_scene: PlanningScene) -> CollisionObject: """ Get a Object from a MoveIt PlanningScene by its name. @@ -260,12 +286,13 @@ def get_obj_from_planning_scene(obj_name: str, planning_scene: PlanningScene) -> raise ValueError(f"Object '{obj_name}' not in planning scene") return planning_scene.get_objects([obj_name])[obj_name] + def obj_to_plane(support_obj: str, planning_scene: PlanningScene, offset: float = 0.001) -> List[Point]: """ Get a surface plane from an object in the MoveIt planning scene. NOTE: Currently only works for boxes - + Parameters ---------- support_obj : str @@ -274,27 +301,33 @@ def obj_to_plane(support_obj: str, planning_scene: PlanningScene, offset: float The MoveIt planning scene. offset : float, optional The offset from the MoveIt object and the created plane. Defaults to 0.001. - + Returns ------- List[Point] - A list of four points (top right, top left, buttom left, buttom right) + A list of four points (top right, top left, buttom left, buttom right) representing the corners of the top surface plane. """ collision_object = get_obj_from_planning_scene(support_obj, planning_scene) if len(collision_object.primitives) != 1 or collision_object.primitives[0].type != 1: raise ValueError(f"Object '{support_obj}' is not a box") - + half_width = collision_object.primitives[0].dimensions[0] / 2 half_depth = collision_object.primitives[0].dimensions[1] / 2 - - corners = [(half_width, half_depth), (-half_width, half_depth), (-half_width, -half_depth), (half_width, -half_depth)] + + corners = [ + (half_width, half_depth), + (-half_width, half_depth), + (-half_width, -half_depth), + (half_width, -half_depth), + ] half_height = collision_object.primitives[0].dimensions[2] / 2 return [Point(dx, dy, half_height + offset) for dx, dy in corners] + def attached_obj_height(attached_obj: str, planning_scene: PlanningScene, offset: float = 0.001) -> float: """ Get the height of an object attached to the gripper @@ -317,30 +350,35 @@ def attached_obj_height(attached_obj: str, planning_scene: PlanningScene, offset dimension_index = 2 if any('power_drill_with_grip' in key for key in planning_scene.get_attached_objects()): dimension_index = 1 - half_height_att_obj = list(planning_scene.get_attached_objects().values())[0].object.primitives[0].dimensions[dimension_index] / 2 + half_height_att_obj = ( + list(planning_scene.get_attached_objects().values())[0].object.primitives[0].dimensions[dimension_index] / 2 + ) return half_height_att_obj + collision_object.primitives[0].dimensions[2] / 2 + offset + # Example usage -if __name__ == '__main__': - rospy.init_node('plane_visualizer', anonymous=False) - support_plane_marker_pub = rospy.Publisher('support_plane_as_marker', Marker, queue_size=1, latch=True) - point_pub = rospy.Publisher('plane_points', PointStamped, queue_size=1, latch=False) - place_poses_pub = rospy.Publisher('~place_poses', ObjectList, queue_size=1, latch=True) - rospy.loginfo('test started') - rospy.sleep(0.2) - - support_object = 'table_3' # the object from which a surface will be generated and later on an object needs to be placed - object_tbp = 'power_drill_with_grip' # the obj class to be place on a surface - plane_1 = obj_to_plane(support_object) - # currently the points need to be specified in a specific order (this is a workaround) - # the animation helps to make sure the order is correct so that the functions can work correctly - animate_plane_points(plane_1, point_pub) - plane_1 = reduce_plane_area(plane_1, -0.2) - # visualize plane as marker - marker_msg = make_plane_marker_msg('map', plane_1) - support_plane_marker_pub.publish(marker_msg) - object_list_msg = gen_place_poses_from_plane(object_tbp, support_object, plane_1) - place_poses_pub.publish(object_list_msg) - - rospy.loginfo('test finished') - rospy.sleep(1.0) +# if __name__ == '__main__': +# rospy.init_node('plane_visualizer', anonymous=False) +# support_plane_marker_pub = rospy.Publisher('support_plane_as_marker', Marker, queue_size=1, latch=True) +# point_pub = rospy.Publisher('plane_points', PointStamped, queue_size=1, latch=False) +# place_poses_pub = rospy.Publisher('~place_poses', ObjectList, queue_size=1, latch=True) +# rospy.loginfo('test started') +# rospy.sleep(0.2) +# +# support_object = ( +# 'table_3' # the object from which a surface will be generated and later on an object needs to be placed +# ) +# object_tbp = 'power_drill_with_grip' # the obj class to be place on a surface +# plane_1 = obj_to_plane(support_object) +# # currently the points need to be specified in a specific order (this is a workaround) +# # the animation helps to make sure the order is correct so that the functions can work correctly +# animate_plane_points(plane_1, point_pub) +# plane_1 = reduce_plane_area(plane_1, -0.2) +# # visualize plane as marker +# marker_msg = make_plane_marker_msg('map', plane_1) +# support_plane_marker_pub.publish(marker_msg) +# object_list_msg = gen_place_poses_from_plane(object_tbp, support_object, plane_1) +# place_poses_pub.publish(object_list_msg) +# +# rospy.loginfo('test finished') +# rospy.sleep(1.0) diff --git a/src/grasplan/visualization/grasp_visualizer.py b/src/grasplan/visualization/grasp_visualizer.py index 8d31390..a718da4 100755 --- a/src/grasplan/visualization/grasp_visualizer.py +++ b/src/grasplan/visualization/grasp_visualizer.py @@ -1,18 +1,18 @@ #!/usr/bin/python3 +''' +Load grasp configurations from yaml file and display them on rviz. +''' + import os import tf import rospy import std_msgs import geometry_msgs -from pose_selector.srv import ClassQuery from geometry_msgs.msg import PoseStamped, PoseArray from visualization_msgs.msg import Marker from grasplan.grasp_planner.handcoded_grasp_planner import HandcodedGraspPlanner -''' -Load grasp configurations from yaml file and display them on rviz. -''' class GraspVisualizer: def __init__(self): @@ -40,7 +40,7 @@ def __init__(self): def UpdateObjectMeshCB(self, msg): self.update_mesh(object_name=msg.data, object_pkg=self.object_pkg) - def make_mesh_marker_msg(self, mesh_path, position=[0,0,0], orientation=[0,0,0,1], mesh_scale=[1,1,1]): + def make_mesh_marker_msg(self, mesh_path, position=[0, 0, 0], orientation=[0, 0, 0, 1], mesh_scale=[1, 1, 1]): mesh_marker_msg = Marker() # mesh_marker_msg.lifetime = rospy.Duration(3.0) mesh_marker_msg.ns = 'object' @@ -56,7 +56,7 @@ def make_mesh_marker_msg(self, mesh_path, position=[0,0,0], orientation=[0,0,0,1 mesh_marker_msg.mesh_use_embedded_materials = True mesh_marker_msg.scale = geometry_msgs.msg.Vector3(mesh_scale[0], mesh_scale[1], mesh_scale[2]) # set rgba to 0 to allow mesh_use_embedded_materials to work - mesh_marker_msg.color = std_msgs.msg.ColorRGBA(0,0,0,0) + mesh_marker_msg.color = std_msgs.msg.ColorRGBA(0, 0, 0, 0) mesh_marker_msg.mesh_resource = mesh_path return mesh_marker_msg @@ -71,8 +71,10 @@ def publish_grasps_as_pose_array(self): object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 object_pose.pose.orientation.w = 1.0 - grasp_type = '' # not implemented yet, so it can be any value - pose_array_msg = self.handcoded_grasp_planner_obj.gen_end_effector_grasp_poses(object_name, object_pose, grasp_type) + grasp_type = '' # not implemented yet, so it can be any value + pose_array_msg = self.handcoded_grasp_planner_obj.gen_end_effector_grasp_poses( + object_name, object_pose, grasp_type + ) self.pose_array_pub.publish(pose_array_msg) def update_mesh(self, object_name='multimeter', object_pkg='mobipick_gazebo'): @@ -85,9 +87,9 @@ def update_mesh(self, object_name='multimeter', object_pkg='mobipick_gazebo'): if mesh_path is None: rospy.logerr('failed to update mesh') return - angular_q = tf.transformations.quaternion_from_euler(self.transform_angular[0],\ - self.transform_angular[1],\ - self.transform_angular[2]) + angular_q = tf.transformations.quaternion_from_euler( + self.transform_angular[0], self.transform_angular[1], self.transform_angular[2] + ) marker_msg = self.make_mesh_marker_msg(mesh_path, position=self.transform_linear, orientation=angular_q) rospy.loginfo(f'publishing mesh:{mesh_path}') self.object_mesh_publisher.publish(marker_msg) @@ -98,6 +100,7 @@ def start_grasp_visualizer(self): self.publish_grasps_as_pose_array() rospy.spin() + if __name__ == '__main__': rospy.init_node('grasp_visualizer', anonymous=False) grasp_visualizer = GraspVisualizer() diff --git a/src/grasplan/visualization/gripper_tf_autom_finder.py b/src/grasplan/visualization/gripper_tf_autom_finder.py index 9497d08..d531804 100755 --- a/src/grasplan/visualization/gripper_tf_autom_finder.py +++ b/src/grasplan/visualization/gripper_tf_autom_finder.py @@ -4,23 +4,30 @@ import tf from urdf_parser_py.urdf import URDF + class GripperFindTransforms: ''' provides information to reconstruct a gripper in rviz using markers ''' + def __init__(self): # parameters robot_description = rospy.get_param('~robot_description', 'mobipick/robot_description') - self.required_links = rospy.get_param('~required_links', ['mobipick/gripper_right_inner_knuckle', - 'mobipick/gripper_right_outer_knuckle', - 'mobipick/gripper_left_inner_knuckle', - 'mobipick/gripper_left_outer_knuckle', - 'mobipick/gripper_left_outer_finger', - 'mobipick/gripper_right_outer_finger', - 'mobipick/gripper_right_inner_finger', - 'mobipick/gripper_left_inner_finger', - 'mobipick/gripper_left_robotiq_fingertip_65mm', - 'mobipick/gripper_right_robotiq_fingertip_65mm']) + self.required_links = rospy.get_param( + '~required_links', + [ + 'mobipick/gripper_right_inner_knuckle', + 'mobipick/gripper_right_outer_knuckle', + 'mobipick/gripper_left_inner_knuckle', + 'mobipick/gripper_left_outer_knuckle', + 'mobipick/gripper_left_outer_finger', + 'mobipick/gripper_right_outer_finger', + 'mobipick/gripper_right_inner_finger', + 'mobipick/gripper_left_inner_finger', + 'mobipick/gripper_left_robotiq_fingertip_65mm', + 'mobipick/gripper_right_robotiq_fingertip_65mm', + ], + ) self.end_effector_link = rospy.get_param('~ee_link', 'mobipick/gripper_tcp') self.tf_attempts = rospy.get_param('~tf_attempts', 3) # the path where to save the configuration in yaml format @@ -30,8 +37,10 @@ def __init__(self): rospy.loginfo('-- parsing urdf --') try: self.robot = URDF.from_parameter_server(robot_description) - except: - rospy.logerr(f'could not get URDF from param server, make sure that the parameter {robot_description} is set') + except KeyError: + rospy.logerr( + f'could not get URDF from param server, make sure that the parameter {robot_description} is set' + ) raise ValueError('could not get URDF from param server') rospy.loginfo('-- end of urdf info--\n') self.listener = tf.TransformListener() @@ -49,8 +58,8 @@ def get_part_tf(self, reference_frame, target_frame): rospy.sleep(0.5) def generate_config_file(self): - f = open(self.yaml_path,'w+') - f.write(f'# This file was automatically generated, manual editing is not recommended\n\n') + f = open(self.yaml_path, 'w+') + f.write('# This file was automatically generated, manual editing is not recommended\n\n') for link in self.robot.links: if link.name in self.required_links: for i, visual in enumerate(link.visuals): @@ -79,7 +88,8 @@ def start_gripper_find_transform(self): rospy.loginfo(f'file generated: {self.yaml_path}') rospy.loginfo('bye bye!') -if __name__=='__main__': + +if __name__ == '__main__': rospy.init_node('gripper_tf_autom_finder_node', anonymous=False) gft = GripperFindTransforms() gft.start_gripper_find_transform() diff --git a/src/grasplan/visualization/rviz_gripper_visualizer.py b/src/grasplan/visualization/rviz_gripper_visualizer.py index f356b2a..a465678 100755 --- a/src/grasplan/visualization/rviz_gripper_visualizer.py +++ b/src/grasplan/visualization/rviz_gripper_visualizer.py @@ -11,18 +11,22 @@ from visualization_msgs.msg import Marker, MarkerArray from geometry_msgs.msg import PoseArray + class GripperVisualiser: ''' Subscribe to geometry_msgs/PoseArray topic for each pose, draws a gripper in rviz using visualization_msgs/MarkerArray each pose is assumed to represent the gripper end effector frame ''' + def __init__(self): # parameters self.global_reference_frame = rospy.get_param('~global_reference_frame', 'map') self.marker_ns = rospy.get_param('~marker_namespace', 'grasp_poses') - self.rgb_mesh_color = rospy.get_param('~rgb_mesh_color', [153.0, 102.0, 51.0]) # use 0.0 - 255.0 range - self.alpha = rospy.get_param('~transparency', 0.5) # the transparency of the mesh, if 1.0 no transparency is set + self.rgb_mesh_color = rospy.get_param('~rgb_mesh_color', [153.0, 102.0, 51.0]) # use 0.0 - 255.0 range + self.alpha = rospy.get_param( + '~transparency', 0.5 + ) # the transparency of the mesh, if 1.0 no transparency is set self.index_of_pose_to_highlight = -1 @@ -46,9 +50,9 @@ def get_tf_pose_array_wrt_global(self, frame_id): (trans, rot) = self.listener.lookupTransform(self.global_reference_frame, frame_id, rospy.Time(0)) euler_rot = tf.transformations.euler_from_quaternion(rot) tf_gripper_to_world = tf.transformations.euler_matrix(euler_rot[0], euler_rot[1], euler_rot[2]) - tf_gripper_to_world[0][3] = trans[0] # x - tf_gripper_to_world[1][3] = trans[1] # y - tf_gripper_to_world[2][3] = trans[2] # z + tf_gripper_to_world[0][3] = trans[0] # x + tf_gripper_to_world[1][3] = trans[1] # y + tf_gripper_to_world[2][3] = trans[2] # z self.tf_gripper_to_world = tf_gripper_to_world return True except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): @@ -68,7 +72,7 @@ def detele_previous_markers(self): def poseArrayCB(self, msg): self.detele_previous_markers() self.mesh_count = 0 - for i in range(5): # try 5 times to query tf + for i in range(5): # try 5 times to query tf if self.get_tf_pose_array_wrt_global(msg.header.frame_id): marker_array_msg = MarkerArray() for i, pose in enumerate(msg.poses): @@ -78,10 +82,12 @@ def poseArrayCB(self, msg): rot.append(pose.orientation.z) rot.append(pose.orientation.w) euler_rot = tf.transformations.euler_from_quaternion(rot) - tf_pose_to_posearrayorigin = tf.transformations.euler_matrix(euler_rot[0], euler_rot[1], euler_rot[2]) - tf_pose_to_posearrayorigin[0][3] = pose.position.x # x - tf_pose_to_posearrayorigin[1][3] = pose.position.y # y - tf_pose_to_posearrayorigin[2][3] = pose.position.z # z + tf_pose_to_posearrayorigin = tf.transformations.euler_matrix( + euler_rot[0], euler_rot[1], euler_rot[2] + ) + tf_pose_to_posearrayorigin[0][3] = pose.position.x # x + tf_pose_to_posearrayorigin[1][3] = pose.position.y # y + tf_pose_to_posearrayorigin[2][3] = pose.position.z # z self.tf_pose_to_posearrayorigin = tf_pose_to_posearrayorigin marker_array_msg = self.extend_marker_array_msg(marker_array_msg, i) self.marker_array_pub.publish(marker_array_msg) @@ -89,7 +95,7 @@ def poseArrayCB(self, msg): else: rospy.sleep(0.3) - def make_marker_msg(self, mesh_path, mesh_scale, position, orientation, color=[0., 255., 0.]): + def make_marker_msg(self, mesh_path, mesh_scale, position, orientation, color=[0.0, 255.0, 0.0]): marker = Marker() # marker.lifetime = rospy.Duration(3.0) marker.ns = self.marker_ns @@ -103,27 +109,30 @@ def make_marker_msg(self, mesh_path, mesh_scale, position, orientation, color=[0 marker.pose.orientation.z = orientation[2] marker.pose.orientation.w = orientation[3] marker.scale = geometry_msgs.msg.Vector3(mesh_scale[0], mesh_scale[1], mesh_scale[2]) - marker.color = std_msgs.msg.ColorRGBA(color[0]/255., color[1]/255., color[2]/255., self.alpha) + marker.color = std_msgs.msg.ColorRGBA(color[0] / 255.0, color[1] / 255.0, color[2] / 255.0, self.alpha) marker.mesh_resource = mesh_path return marker def transform_part_to_gripper_ref_frame(self, t1, r1, mesh_translation, mesh_rotation): r1_euler = tf.transformations.euler_from_quaternion(r1) tf_mesh_to_gripper = tf.transformations.euler_matrix(r1_euler[0], r1_euler[1], r1_euler[2]) - tf_mesh_to_gripper[0][3] = t1[0] # x - tf_mesh_to_gripper[1][3] = t1[1] # y - tf_mesh_to_gripper[2][3] = t1[2] # z + tf_mesh_to_gripper[0][3] = t1[0] # x + tf_mesh_to_gripper[1][3] = t1[1] # y + tf_mesh_to_gripper[2][3] = t1[2] # z tf_part_to_mesh = tf.transformations.euler_matrix(mesh_rotation[0], mesh_rotation[1], mesh_rotation[2]) - tf_part_to_mesh[0][3] = mesh_translation[0] # x - tf_part_to_mesh[1][3] = mesh_translation[1] # y - tf_part_to_mesh[2][3] = mesh_translation[2] # z + tf_part_to_mesh[0][3] = mesh_translation[0] # x + tf_part_to_mesh[1][3] = mesh_translation[1] # y + tf_part_to_mesh[2][3] = mesh_translation[2] # z # transform once more to global reference frame - new_m = np.dot(self.tf_gripper_to_world, (np.dot(self.tf_pose_to_posearrayorigin, np.dot(tf_mesh_to_gripper, tf_part_to_mesh)))) + new_m = np.dot( + self.tf_gripper_to_world, + (np.dot(self.tf_pose_to_posearrayorigin, np.dot(tf_mesh_to_gripper, tf_part_to_mesh))), + ) n_roll, n_pitch, n_yaw = tf.transformations.euler_from_matrix(new_m) q_orientation = tf.transformations.quaternion_from_euler(n_roll, n_pitch, n_yaw) - position = [new_m[0][3], new_m[1][3], new_m[2][3] ] + position = [new_m[0][3], new_m[1][3], new_m[2][3]] return position, q_orientation def extend_marker_array_msg(self, marker_array_msg, count): @@ -136,21 +145,30 @@ def extend_marker_array_msg(self, marker_array_msg, count): mesh_rotation = link_dic[key][visual]['mesh_rotation'] mesh_path = link_dic[key][visual]['mesh_path'] mesh_scale = link_dic[key][visual]['mesh_scale'] - position, q_orientation = self.transform_part_to_gripper_ref_frame(tf_translation, tf_rotation, mesh_translation, mesh_rotation) + position, q_orientation = self.transform_part_to_gripper_ref_frame( + tf_translation, tf_rotation, mesh_translation, mesh_rotation + ) # highlight a certain grasp number in green color if self.index_of_pose_to_highlight == -10: - marker_msg = self.make_marker_msg(mesh_path, mesh_scale, position, q_orientation, color=[0., 255., 0.]) + marker_msg = self.make_marker_msg( + mesh_path, mesh_scale, position, q_orientation, color=[0.0, 255.0, 0.0] + ) else: if count == self.index_of_pose_to_highlight: - marker_msg = self.make_marker_msg(mesh_path, mesh_scale, position, q_orientation, color=[0., 255., 0.]) + marker_msg = self.make_marker_msg( + mesh_path, mesh_scale, position, q_orientation, color=[0.0, 255.0, 0.0] + ) else: - marker_msg = self.make_marker_msg(mesh_path, mesh_scale, position, q_orientation, color=self.rgb_mesh_color) + marker_msg = self.make_marker_msg( + mesh_path, mesh_scale, position, q_orientation, color=self.rgb_mesh_color + ) marker_msg.id = self.mesh_count self.mesh_count += 1 marker_array_msg.markers.append(copy.deepcopy(marker_msg)) return marker_array_msg -if __name__=='__main__': + +if __name__ == '__main__': rospy.init_node('gripper_visualizer_node', anonymous=False) gv = GripperVisualiser() rospy.spin()