From c6fe263e1e6e4615894476fa077aada1034e0b08 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Thu, 12 Oct 2023 17:29:33 +0900 Subject: [PATCH] obinata wip --- jsk_fetch_robot/jsk_fetch_startup/package.xml | 91 ++++---- .../scripts/moveit_noetic_bridge.py | 194 +++++++++++++++--- 2 files changed, 210 insertions(+), 75 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch_startup/package.xml b/jsk_fetch_robot/jsk_fetch_startup/package.xml index a89d0c4dea..6dfc1e487e 100644 --- a/jsk_fetch_robot/jsk_fetch_startup/package.xml +++ b/jsk_fetch_robot/jsk_fetch_startup/package.xml @@ -1,4 +1,4 @@ - + jsk_fetch_startup 1.1.0 @@ -13,59 +13,62 @@ fetch_teleop - image_proc - jsk_fetch_accessories - jsk_fetch_diagnosis - jsk_network_tools - jsk_robot_startup - jsk_tools - jsk_maps - fetcheus - fetch_navigation - fetch_moveit_config - fetch_open_auto_dock - fetch_driver_msgs - nodelet - pr2_computer_monitor - robot_pose_publisher - rviz - tf - touchegg - sound_play - switchbot_ros - tf2_ros - voice_text + image_proc + jsk_fetch_accessories + jsk_fetch_diagnosis + jsk_network_tools + jsk_robot_startup + jsk_tools + jsk_maps + fetcheus + fetch_navigation + fetch_moveit_config + fetch_open_auto_dock + fetch_driver_msgs + nodelet + pr2_computer_monitor + robot_pose_publisher + rviz + tf + touchegg + sound_play + switchbot_ros + tf2_ros + voice_text - amcl - global_planner - map_server - move_base - move_slow_and_clear - robot_localization - teb_local_planner - pyquaternion - librealsense2 - realsense2_camera - realsense2_description + amcl + global_planner + map_server + move_base + move_slow_and_clear + robot_localization + teb_local_planner + pyquaternion + librealsense2 + realsense2_camera + realsense2_description - app_manager - fetch_bringup - safe_teleop_base - joy - topic_tools - fetch_teleop + app_manager + fetch_bringup + safe_teleop_base + joy + topic_tools + fetch_teleop - diagnostic_aggregator + diagnostic_aggregator - julius_ros - respeaker_ros + julius_ros + respeaker_ros - robot_pose_publisher + robot_pose_publisher + + python-packaging + python3-packaging rostest diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py index c9baac1a38..2c9be06c30 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py +++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py @@ -1,9 +1,24 @@ #!/usr/bin/env python +import importlib.util +import rospkg import rospy -from moveit_msgs.msg import CollisionObject, RobotState, AttachedCollisionObject +from moveit_msgs.msg import ( + AttachedCollisionObject, + CollisionObject, + RobotState, +) # importing noetic version +from moveit_msgs.srv import ( + GetMotionPlan, + GetMotionPlanRequest, + GetPlanningScene, + GetPlanningSceneRequest, + GetPlanningSceneResponse, + GetStateValidity, + GetStateValidityRequest, +) # importing noetic version +from packaging import version from rospy.impl.tcpros_pubsub import check_if_still_publisher -from moveit_msgs.srv import GetPlanningScene, GetPlanningSceneRequest, GetPlanningSceneResponse """ @@ -72,7 +87,7 @@ * /set_planner_params ``` -and the topic depends on CollisionObjects are +and the topics / services depend on CollisionObjects are ``` /apply_planning_scene moveit_msgs/ApplyPlanningScene /check_state_validity moveit_msgs/GetStateValidity @@ -89,50 +104,156 @@ class MoveitNoeticBridge(object): """ This node is expected to work on melodic PC """ + def __init__(self): - self._srv_md5sum = { - "moveit_msgs/GetPlanningScene": { - "noetic": "65cf69d3e7a0342e16374024d4eeef65", - "melodic": "2745cf315b4eb5fb00e5befa8714d64d", - }, - "moveit_msgs/GetMotionPlan": { - "noetic": "110dffa73f82b9b8644f525e7fbd9fc4", - "melodic": "657e571ceabcb225c850c02c2249a1e1", - }, - "moveit_msgs/GetStateValidity": { - "noetic": "06ea62db671e4dbf878eaca241db51ad", - "melodic": "0c7c937b6a056e7ae5fded13d8e9b242", - }, - } + # Loading Melodic version services, c.f. https://stackoverflow.com/questions/67631/how-can-i-import-a-module-dynamically-given-the-full-path + self._apply_planning_scene_melodic_spec = importlib.util.spec_from_file_location( + "ApplyPlanningScene", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_ApplyPlanningScene.py", + ) + self._get_motion_plan_melodic_spec = importlib.util.spec_from_file_location( + "GetMotionPlan", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetMotionPlan.py", + ) + self._get_state_validity_melodic_spec = importlib.util.spec_from_file_location( + "GetStateValidity", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetStateValidity.py", + ) + self._melodic_apply_planning_scene = importlib.util.module_from_spec( + self._apply_planning_scene_melodic_spec + ) + self._melodic_plan_kinematic_path = importlib.util.module_from_spec( + self._get_motion_plan_melodic_spec + ) + self._melodic_check_state_validity = importlib.util.module_from_spec( + self._get_state_validity_melodic_spec + ) + self._apply_planning_scene_melodic_spec.loader.exec_state( + self._melodic_apply_planning_scene + ) + self._get_motion_plan_melodic_spec.loader.exec_state( + self._melodic_plan_kinematic_path + ) + self._get_state_validity_melodic_spec.loader.exec_state( + self._melodic_check_state_validity + ) # Service bridge self.get_planning_scene_srv = rospy.Service( - "/get_planning_scene", rospy.AnyMsg, self._srv_cb + "/get_planning_scene_noetic", GetPlanningScene, self._srv_cb ) self.plan_kinematic_path_srv = rospy.Service( - "/plan_kinematic_path", rospy.AnyMsg, self._srv_cb + "/plan_kinematic_path_noetic", GetMotionPlan, self._srv_cb ) self.check_state_validity_srv = rospy.Service( - "/check_state_validity", rospy.AnyMsg, self._srv_cb + "/check_state_validity_noetic", GetStateValidity, self._srv_cb ) - def _srv_cb(self, request: rospy.AnyMsg): + # Service proxy + self.get_planning_scene_proxy = rospy.ServiceProxy( + "/get_planning_scene", GetPlanningScene + ) + self.plan_kinematic_path_proxy = rospy.ServiceProxy( + "/plan_kinematic_path", GetMotionPlan + ) + self.check_state_validity_proxy = rospy.ServiceProxy( + "/check_state_validity", GetStateValidity + ) + + def _srv_cb(self, request): """ :param request: service request - :type msg: rospy.AnyMsg + :type request: :returns: None :rtype: None """ - if not self._is_msg_noetic(request): - rospy.logdebug("The message is not noetic version") - return if request._type == "moveit_msgs/GetPlanningScene": - bridged_request = GetPlanningSceneRequest() - bridged_request.name = request.name + bridged_request = ( + self._melodic_apply_planning_scene.ApplyPlanningSceneRequest() + ) + bridged_request.scene.name = request.scene.name + bridged_request.scene.robot_state = self._convert_noetic_robot_state_msg( + request.scene.robot_state + ) + bridged_request.scene.robot_model_name = request.scene.robot_model_name + bridged_request.scene.fixed_frame_transforms = ( + request.scene.fixed_frame_transforms + ) + bridged_request.scene.allowed_collision_matrix = ( + request.scene.allowed_collision_matrix + ) + bridged_request.scene.link_padding = request.scene.link_padding + bridged_request.scene.link_scale = request.scene.link_scale + bridged_request.scene.object_colors = request.scene.object_colors + bridged_request.scene.world.collision_objects = ( + self._convert_noetic_collision_object_msg( + request.scene.world.collision_objects + ) + ) + bridged_request.scene.world.octomap = ( + self._convert_noetic_collision_object_msg(request.scene.world.octomap) + ) + bridged_request.scene.is_diff = request.scene.is_diff + res = self.get_planning_scene_proxy.call(bridged_request) + return res elif request._type == "moveit_msgs/GetMotionPlan": + bridged_request = ( + self._melodic_plan_kinematic_path.PlanKinematicPathRequest() + ) + bridged_request.motion_plan_request.workspace_parameters = ( + request.motion_plan_request.workspace_parameters + ) + bridged_request.motion_plan_request.start_state = ( + self._convert_noetic_robot_state_msg( + request.motion_plan_request.start_state + ) + ) + bridged_request.motion_plan_request.goal_constraints = ( + request.motion_plan_request.goal_constraints + ) + bridged_request.motion_plan_request.path_constraints = ( + request.motion_plan_request.path_constraints + ) + bridged_request.motion_plan_request.trajectory_constraints = ( + request.motion_plan_request.trajectory_constraints + ) + bridged_request.motion_plan_request.reference_trajectories = ( + request.motion_plan_request.reference_trajectories + ) + bridged_request.motion_plan_request.pipeline_id = ( + request.motion_plan_request.pipeline_id + ) + bridged_request.motion_plan_request.planner_id = ( + request.motion_plan_request.planner_id + ) + bridged_request.motion_plan_request.group_name = ( + request.motion_plan_request.group_name + ) + bridged_request.motion_plan_request.num_planning_attempts = ( + request.motion_plan_request.num_planning_attempts + ) + bridged_request.motion_plan_request.allowed_planning_time = ( + request.motion_plan_request.allowed_planning_time + ) + bridged_request.motion_plan_request.max_velocity_scaling_factor = ( + request.motion_plan_request.max_velocity_scaling_factor + ) + bridged_request.motion_plan_request.max_acceleration_scaling_factor = ( + request.motion_plan_request.max_acceleration_scaling_factor + ) + bridged_request.motion_plan_request.cartesian_speed_limited_link = ( + request.motion_plan_request.cartesian_speed_limited_link + ) + bridged_request.motion_plan_request.max_cartesian_speed = ( + request.motion_plan_request.max_cartesian_speed + ) + res = self.plan_kinematic_path_proxy.call( + bridged_request + ) # TODO need conversion response pass + elif request._type == "moveit_msgs/GetStateValidity": - pass + bridged_request = self._melodic_check_state_validity.CheckStateValidity() def _is_msg_noetic(self, msg): """ @@ -157,7 +278,9 @@ def _convert_noetic_robot_state_msg(self, robot_state_msg): for obj in robot_state_msg.attached_collision_objects: melodic_collision_object = AttachedCollisionObject() melodic_collision_object.link_name = obj.link_name - melodic_collision_object.object = self._convert_noetic_collision_object_msg(obj.object) + melodic_collision_object.object = self._convert_noetic_collision_object_msg( + obj.object + ) melodic_collision_object.touch_links = obj.touch_links melodic_collision_object.detach_posture = obj.detach_posture melodic_collision_object.weight = obj.weight @@ -184,7 +307,6 @@ def _convert_melodic_collision_object_msg(self, collision_object_msg): melodic_msg.operation = collision_object_msg.operation return melodic_msg - def _convert_noetic_collision_object_msg(self, collision_object_msg): """ :param collision_object_msg: Noetic CollisionObject message @@ -206,7 +328,17 @@ def _convert_noetic_collision_object_msg(self, collision_object_msg): return melodic_msg -if __name__ == "__main__": +def main(): + r = rospkg.RosPack() + moveit_msgs_manifest = r.get_manifest("moveit_msgs") + current_version = moveit_msgs_manifest.version + if version.parse(current_version) > version.parse("0.11.0"): + rospy.logfatal("Unsupported version") + return rospy.init_node("moveit_noetic_bridge") bridge_node = MoveitNoeticBridge() rospy.spin() + + +if __name__ == "__main__": + main()