From 7c5318d58f93dd2564cb4fe8c65429b75212c714 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Tue, 26 Sep 2023 17:39:15 +0900 Subject: [PATCH 01/10] [jsk_fetch_startup][noetic] moveit bridge for noetic client --- jsk_fetch_robot/jsk_fetch_startup/package.xml | 91 +-- .../scripts/moveit_noetic_bridge.py | 555 ++++++++++++++++++ 2 files changed, 602 insertions(+), 44 deletions(-) create mode 100755 jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py 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 new file mode 100755 index 0000000000..b3611019fa --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py @@ -0,0 +1,555 @@ +#!/usr/bin/env python + +import imp +import rospkg +import rospy +from moveit_msgs.msg import ( + AttachedCollisionObject, + CollisionObject, + MotionPlanResponse, + PlanningScene, + PlanningSceneWorld, + RobotState, +) # importing noetic version +from moveit_msgs.srv import ( + ApplyPlanningScene, + GetMotionPlan, + GetPlanningScene, + GetStateValidity, + GetPositionIK, +) # importing noetic version +from packaging import version +from rospy.impl.tcpros_pubsub import check_if_still_publisher + + +""" +On https://github.com/ros-planning/moveit_msgs/compare/0.10.1...0.11.4, the CollisionObject.msg has breaking changes +It affects the msgs, AttachedCollisionObject.msg, PlanningSceneWorld.msg and RobotState.msg. + +In fetch, these moveit_msgs's topics are advertised +``` +obinata@fetch1075:~ $ rostopic list -v | grep moveit_msgs + * /place/result [moveit_msgs/PlaceActionResult] 1 publisher + * /move_group/monitored_planning_scene [moveit_msgs/PlanningScene] 1 publisher + * /pickup/feedback [moveit_msgs/PickupActionFeedback] 1 publisher + * /move_group/feedback [moveit_msgs/MoveGroupActionFeedback] 1 publisher + * /execute_trajectory/feedback [moveit_msgs/ExecuteTrajectoryActionFeedback] 1 publisher + * /place/feedback [moveit_msgs/PlaceActionFeedback] 1 publisher + * /pickup/result [moveit_msgs/PickupActionResult] 1 publisher + * /move_group/display_planned_path [moveit_msgs/DisplayTrajectory] 1 publisher + * /planning_scene_world [moveit_msgs/PlanningSceneWorld] 1 publisher + * /move_group/result [moveit_msgs/MoveGroupActionResult] 1 publisher + * /execute_trajectory/result [moveit_msgs/ExecuteTrajectoryActionResult] 1 publisher + * /collision_object [moveit_msgs/CollisionObject] 1 subscriber + * /planning_scene_world [moveit_msgs/PlanningSceneWorld] 1 subscriber + * /move_group/goal [moveit_msgs/MoveGroupActionGoal] 1 subscriber + * /place/goal [moveit_msgs/PlaceActionGoal] 1 subscriber + * /attached_collision_object [moveit_msgs/AttachedCollisionObject] 1 subscriber + * /pickup/goal [moveit_msgs/PickupActionGoal] 1 subscriber + * /execute_trajectory/goal [moveit_msgs/ExecuteTrajectoryActionGoal] 1 subscriber + * /planning_scene [moveit_msgs/PlanningScene] 1 subscriber +``` +and the topic depends on CollisionObjects are +``` +/place moveit_msgs/PlaceAction +/move_group/monitored_planning_scene moveit_msgs/PlanningScene +/pickup moveit_msgs/PickupAction +/move_group moveit_msgs/MoveGroupAction +/move_group/display_planned_path moveit_msgs/DisplayTrajectory +/planning_scene_world moveit_msgs/PlanningSceneWorld +/collision_object moveit_msgs/CollisionObject +/attached_collision_object moveit_msgs/AttachedCollisionObject +/planning_scene moveit_msgs/PlanningSceneWorld +``` + +these moveit_msgs's services may be advertised +``` +obinata@fetch1075:/opt/ros/melodic/share/moveit_core $ rosnode info /move_group -q +... +Services + * /apply_planning_scene + * /check_state_validity + * /clear_octomap + * /compute_cartesian_path + * /compute_fk + * /compute_ik + * /get_planner_params + * /get_planning_scene + * /move_group/get_loggers + * /move_group/load_map + * /move_group/ompl/set_parameters + * /move_group/plan_execution/set_parameters + * /move_group/planning_scene_monitor/set_parameters + * /move_group/save_map + * /move_group/sense_for_plan/set_parameters + * /move_group/set_logger_level + * /move_group/trajectory_execution/set_parameters + * /plan_kinematic_path + * /query_planner_interface + * /set_planner_params + +``` +and the services depend on CollisionObjects are +``` +/apply_planning_scene moveit_msgs/ApplyPlanningScene +/check_state_validity moveit_msgs/GetStateValidity +/compute_cartesian_path moveit_msgs/GetCartesianPath +/compute_fk moveit_msgs/GetPositionFK +/compute_ik moveit_msgs/GetPositionIK +/get_planning_scene moveit_msgs/GetPlanningScene +/plan_kinematic_path moveit_msgs/GetMotionPlan +``` +""" + + +class MoveitNoeticBridge(object): + """ + This node is expected to work on melodic PC + """ + + def __init__(self): + # Loading Melodic version services, c.f. https://stackoverflow.com/questions/67631/how-can-i-import-a-module-dynamically-given-the-full-path + self._melodic_apply_planning_scene = imp.load_source( + "ApplyPlanningScene", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_ApplyPlanningScene.py", + ) + self._melodic_get_motion_plan = imp.load_source( + "GetMotionPlan", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetMotionPlan.py", + ) + self._melodic_get_state_validity = imp.load_source( + "GetStateValidity", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetStateValidity.py", + ) + self._melodic_get_position_ik = imp.load_source( + "GetPositionIK", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPositionIK.py", + ) + self._melodic_get_planning_scene = imp.load_source( + "GetPlanningScene", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPlanningScene.py", + ) + + # Loading Melodic version messages + self._melodic_collision_object = imp.load_source( + "CollisionObject", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_CollisionObject.py", + ) + self._melodic_attached_collision_object = imp.load_source( + "AttachedCollisionObject", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_AttachedCollisionObject.py", + ) + self._melodic_motion_plan_request = imp.load_source( + "MotionPlanRequest", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MotionPlanRequest.py", + ) + self._melodic_position_ik_request = imp.load_source( + "PositionIKRequest", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PositionIKRequest.py", + ) + self._melodic_planning_scene = imp.load_source( + "PlanningScene", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningScene.py", + ) + self._melodic_planning_scene_world = imp.load_source( + "PlanningSceneWorld", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningSceneWorld.py", + ) + self._melodic_robot_state = imp.load_source( + "RobotState", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_RobotState.py", + ) + + # Service bridge + self.apply_planning_scene_srv = rospy.Service( + "/apply_planning_scene_noetic", + ApplyPlanningScene, + self._apply_planning_scene_srv_cb, + ) + self.check_state_validity_srv = rospy.Service( + "/check_state_validity_noetic", + GetStateValidity, + self._check_state_validity_srv_cb, + ) + self.compute_ik_srv = rospy.Service( + "/compute_ik_noetic", GetPositionIK, self._compute_ik_srv_cb + ) + self.get_planning_scene_srv = rospy.Service( + "/get_planning_scene_noetic", + GetPlanningScene, + self._get_planning_scene_srv_cb, + ) + self.plan_kinematic_path_srv = rospy.Service( + "/plan_kinematic_path_noetic", + GetMotionPlan, + self._plan_kinematic_path_srv_cb, + ) # NOTE Is this service necessary to be bridged? Not seen in pr2eus_moveit + + # Service proxy + self.apply_planning_scene_proxy = rospy.ServiceProxy( + "/apply_planning_scene", + self._melodic_apply_planning_scene.ApplyPlanningScene, + ) + self.check_state_validity_proxy = rospy.ServiceProxy( + "/check_state_validity", self._melodic_get_state_validity.GetStateValidity + ) + self.compute_ik_proxy = rospy.ServiceProxy( + "/compute_ik", self._melodic_get_position_ik.GetPositionIK + ) + self.get_planning_scene_proxy = rospy.ServiceProxy( + "/get_planning_scene", self._melodic_get_planning_scene.GetPlanningScene + ) + self.plan_kinematic_path_proxy = rospy.ServiceProxy( + "/plan_kinematic_path", self._melodic_get_motion_plan.GetMotionPlan + ) + + def _apply_planning_scene_srv_cb(self, request): + bridged_request = self._melodic_apply_planning_scene.ApplyPlanningSceneRequest() + bridged_request.scene = self._convert_noetic_planning_scene_msg_to_melodic( + request.scene + ) + original_response = self.get_planning_scene_proxy.call(bridged_request) + response = self._melodic_apply_planning_scene.ApplyPlanningSceneResponse() + response.success = original_response.success + return response + + def _check_state_validity_srv_cb(self, request): + bridged_request = self._melodic_get_state_validity.GetStateValidityRequest() + bridged_request.robot_state = self._convert_noetic_robot_state_msg_to_melodic( + request.robot_state + ) + bridged_request.group_name = request.group_name + bridged_request.constraints = request.constraints + original_response = self.check_state_validity_proxy.call(bridged_request) + response = self._melodic_get_state_validity.GetStateValidityResponse() + response.valid = original_response.valid + response.contacts = original_response.contacts + response.cost_sources = original_response.cost_sources + response.constraints = original_response.constraints + return response + + def _compute_ik_srv_cb(self, request): + bridged_request = self._melodic_get_position_ik.GetPositionIKRequest() + bridged_request.ik_request = ( + self._convert_noetic_position_ik_request_msg_to_melodic(request.ik_request) + ) + original_response = self.compute_ik_proxy.call(bridged_request) + response = self._melodic_get_position_ik.GetPositionIKResponse() + response.solution = self._convert_melodic_robot_state_msg_to_noetic( + original_response.solution + ) + response.error_code = original_response.error_code + return response + + def _get_planning_scene_srv_cb(self, request): + bridged_request = self._melodic_get_planning_scene.GetPlanningSceneRequest() + bridged_request.components = request.components + original_response = self.get_planning_scene_proxy.call(bridged_request) + response = self._melodic_get_planning_scene.GetPlanningSceneResponse() + response.scene = self._convert_melodic_planning_scene_msg_to_noetic( + original_response.scene + ) + return response + + def _plan_kinematic_path_srv_cb(self, request): + bridged_request = self._melodic_get_motion_plan.GetMotionPlanRequest() + bridged_request.motion_plan_request = ( + self._convert_noetic_motion_plan_request_msg_to_melodic( + request.motion_plan_request + ) + ) + original_response = self.plan_kinematic_path_proxy.call(bridged_request) + response = self._melodic_get_motion_plan.GetMotionPlanResponse() + response.motion_plan_response = ( + self._convert_melodic_motion_plan_response_msg_to_noetic( + original_response.motion_plan_response + ) + ) + return response + + def _convert_melodic_collision_object_msg_to_noetic(self, collision_object_msg): + """ + :param collision_object_msg: Melodic CollisionObject message + :type colision_object_msg: moveit_msgs.msg.CollisionObject + :returns: Noetic CollisionObject message + :rtype: moveit_msgs.msg.CollisionObject + """ + noetic_msg = CollisionObject() + noetic_msg.header = collision_object_msg.header + noetic_msg.id = collision_object_msg.id + noetic_msg.type = collision_object_msg.type + noetic_msg.primitives = collision_object_msg.primitives + noetic_msg.primitive_poses = collision_object_msg.primitive_poses + noetic_msg.meshes = collision_object_msg.meshes + noetic_msg.mesh_poses = collision_object_msg.mesh_poses + noetic_msg.planes = collision_object_msg.planes + noetic_msg.plane_poses = collision_object_msg.plane_poses + noetic_msg.operation = collision_object_msg.operation + # NOTE The field subframe_names, subframe_poses and pose are ignored + return noetic_msg + + def _convert_noetic_collision_object_msg_to_melodic(self, collision_object_msg): + """ + :param collision_object_msg: Noetic CollisionObject message + :type colision_object_msg: moveit_msgs.msg.CollisionObject + :returns: Melodic CollisionObject message + :rtype: moveit_msgs.msg.CollisionObject + """ + melodic_msg = self._melodic_collision_object.CollisionObject() + melodic_msg.header = collision_object_msg.header + melodic_msg.id = collision_object_msg.id + melodic_msg.type = collision_object_msg.type + melodic_msg.primitives = collision_object_msg.primitives + melodic_msg.primitive_poses = collision_object_msg.primitive_poses + melodic_msg.meshes = collision_object_msg.meshes + melodic_msg.mesh_poses = collision_object_msg.mesh_poses + melodic_msg.planes = collision_object_msg.planes + melodic_msg.plane_poses = collision_object_msg.plane_poses + melodic_msg.operation = collision_object_msg.operation + # NOTE The field subframe_names, subframe_poses and pose are ignored + return melodic_msg + + def _convert_melodic_attached_collision_object_msg_to_noetic( + self, attached_collision_object_msg + ): + """ + :param attached_collision_object_msg: Melodic AttachedCollisionObject message + :type attached_colision_object_msg: moveit_msgs.msg.AttachedCollisionObject + :returns: Noetic AttachedCollisionObject message + :rtype: moveit_msgs.msg.AttachedCollisionObject + """ + noetic_msg = AttachedCollisionObject() + noetic_msg.link_name = attached_collision_object_msg.link_name + noetic_msg.object = self._convert_melodic_collision_object_msg_to_noetic( + attached_collision_object_msg.object + ) + noetic_msg.touch_links = attached_collision_object_msg.touch_links + noetic_msg.detach_posture = attached_collision_object_msg.detach_posture + noetic_msg.weight = attached_collision_object_msg.weight + return noetic_msg + + def _convert_noetic_attached_collision_object_msg_to_melodic( + self, attached_collision_object_msg + ): + """ + :param attached_collision_object_msg: Noetic AttachedCollisionObject message + :type attached_colision_object_msg: moveit_msgs.msg.AttachedCollisionObject + :returns: Melodic AttachedCollisionObject message + :rtype: moveit_msgs.msg.AttachedCollisionObject + """ + melodic_msg = self._melodic_attached_collision_object.AttachedCollisionObject() + melodic_msg.link_name = attached_collision_object_msg.link_name + melodic_msg.object = self._convert_noetic_collision_object_msg_to_melodic( + attached_collision_object_msg.object + ) + melodic_msg.touch_links = attached_collision_object_msg.touch_links + melodic_msg.detach_posture = attached_collision_object_msg.detach_posture + melodic_msg.weight = attached_collision_object_msg.weight + return melodic_msg + + def _convert_noetic_motion_plan_request_msg_to_melodic( + self, motion_plan_request_msg + ): + """ + :param motion_plan_request_msg: Noetic MotionPlanRequest message + :type motion_plan_request_msg: moveit_msgs.msg.MotionPlanRequest + :returns: Noetic MotionPlanRequest message + :rtype: moveit_msgs.msg.MotionPlanRequest + """ + melodic_msg = self._melodic_motion_plan_request.MotionPlanRequest() + melodic_msg.workspace_parameters = motion_plan_request_msg.workspace_parameters + melodic_msg.start_state = self._convert_noetic_robot_state_msg_to_melodic( + motion_plan_request_msg.start_state + ) + melodic_msg.goal_constraints = motion_plan_request_msg.goal_constraints + melodic_msg.path_constraints = motion_plan_request_msg.path_constraints + melodic_msg.trajectory_constraints = ( + motion_plan_request_msg.trajectory_constraints + ) + melodic_msg.planner_id = motion_plan_request_msg.planner_id + melodic_msg.group_name = motion_plan_request_msg.group_name + melodic_msg.num_planning_attempts = ( + motion_plan_request_msg.num_planning_attempts + ) + melodic_msg.allowed_planning_time = ( + motion_plan_request_msg.allowed_planning_time + ) + melodic_msg.max_velocity_scaling_factor = ( + motion_plan_request_msg.max_velocity_scaling_factor + ) + melodic_msg.max_acceleration_scaling_factor = ( + motion_plan_request_msg.max_acceleration_scaling_factor + ) + return melodic_msg + + def _convert_melodic_motion_plan_response_msg_to_noetic( + self, motion_plan_response_msg + ): + noetic_msg = MotionPlanResponse() + noetic_msg.trajectory_start = self._convert_melodic_robot_state_msg_to_noetic( + motion_plan_response_msg.trajectory_start + ) + noetic_msg.group_name = motion_plan_response_msg.group_name + noetic_msg.trajectory = motion_plan_response_msg.trajectory + noetic_msg.planning_time = motion_plan_response_msg.planning_time + noetic_msg.error_code = motion_plan_response_msg.error_code + + def _convert_noetic_position_ik_request_msg_to_melodic( + self, position_ik_request_msg + ): + """ + :param position_ik_request: Noetic PositionIKRequest message + :type position_ik_request: moveit_msgs.msg.PositionIKRequest + :returns: Melodic PositionIKRequest message + :rtype: moveit_msgs.msg.PositionIKRequest + """ + melodic_msg = self._melodic_position_ik_request.PositionIKRequest() + melodic_msg.group_name = position_ik_request_msg.group_name + melodic_msg.robot_state = self._convert_noetic_robot_state_msg_to_melodic( + position_ik_request_msg.robot_state + ) + melodic_msg.constraints = position_ik_request_msg.constraints + melodic_msg.avoid_collisios = position_ik_request_msg.avoid_collisions + melodic_msg.ik_link_name = position_ik_request_msg.ik_link_name + melodic_msg.pose_stamped = position_ik_request_msg.pose_stamped + melodic_msg.ik_link_names = position_ik_request_msg.ik_link_names + melodic_msg.pose_stamped_vector = position_ik_request_msg.pose_stamped_vector + melodic_msg.timeout = position_ik_request_msg.timeout + melodic_msg.attempts = position_ik_request_msg.attempts + return melodic_msg + + def _convert_melodic_planning_scene_msg_to_noetic(self, planning_scene_msg): + """ + :param planning_scene_msg: Melodic PlanningScene message + :type planning_scene_msg: moveit_msgs.msg.PlanningScene + :returns: Noetic PlanningScene message + :rtype: moveit_msgs.msg.PlanningScene + """ + noetic_msg = PlanningScene() + noetic_msg.name = planning_scene_msg.name + noetic_msg.robot_state = self._convert_melodic_robot_state_msg_to_noetic( + planning_scene_msg.robot_state + ) + noetic_msg.robot_model_name = planning_scene_msg.robot_model_name + noetic_msg.fixed_frame_transforms = planning_scene_msg.fixed_frame_transforms + noetic_msg.allowed_collision_matrix = ( + planning_scene_msg.allowed_collision_matrix + ) + noetic_msg.link_padding = planning_scene_msg.link_padding + noetic_msg.link_scale = planning_scene_msg.link_scale + noetic_msg.object_colors = planning_scene_msg.object_colors + noetic_msg.world = self._convert_melodic_planning_scene_world_msg_to_noetic( + planning_scene_msg.world + ) + noetic_msg.is_diff = planning_scene_msg.is_diff + return noetic_msg + + def _convert_noetic_planning_scene_msg_to_melodic(self, planning_scene_msg): + """ + :param planning_scene_msg: Noetic PlanningScene message + :type planning_scene_msg: moveit_msgs.msg.PlanningScene + :returns: Melodic PlanningScene message + :rtype: moveit_msgs.msg.PlanningScene + """ + melodic_msg = self._melodic_planning_scene.PlanningScene() + melodic_msg.name = planning_scene_msg.name + melodic_msg.robot_state = self._convert_noetic_robot_state_msg_to_melodic( + planning_scene_msg.robot_state + ) + melodic_msg.robot_model_name = planning_scene_msg.robot_model_name + melodic_msg.fixed_frame_transforms = planning_scene_msg.fixed_frame_transforms + melodic_msg.allowed_collision_matrix = ( + planning_scene_msg.allowed_collision_matrix + ) + melodic_msg.link_padding = planning_scene_msg.link_padding + melodic_msg.link_scale = planning_scene_msg.link_scale + melodic_msg.object_colors = planning_scene_msg.object_colors + melodic_msg.world = self._convert_noetic_planning_scene_world_msg_to_melodic( + planning_scene_msg.world + ) + melodic_msg.is_diff = planning_scene_msg.is_diff + return melodic_msg + + def _convert_melodic_planning_scene_world_msg_to_noetic( + self, planning_scene_world_msg + ): + """ + :param planning_scene_world_msg: Melodic PlanningSceneWorld message + :type planning_scene_world_msg: moveit_msgs.msg.PlanningSceneWorld + :returns: Noetic PlanningSceneWorld message + :rtype: moveit_msgs.msg.PlanningSceneWorld + """ + noetic_msg = PlanningSceneWorld() + for obj in planning_scene_world_msg.collision_objects: + noetic_msg.collision_objects.append( + self._convert_melodic_collision_object_msg_to_noetic(obj) + ) + noetic_msg.octomap = planning_scene_world_msg.octomap + return noetic_msg + + def _convert_noetic_planning_scene_world_msg_to_melodic( + self, planning_scene_world_msg + ): + """ + :param planning_scene_world_msg: Noetic PlanningSceneWorld message + :type planning_scene_world_msg: moveit_msgs.msg.PlanningSceneWorld + :returns: Melodic PlanningSceneWorld message + :rtype: moveit_msgs.msg.PlanningSceneWorld + """ + melodic_msg = self._melodic_planning_scene_world.PlanningSceneWorld() + for obj in planning_scene_world_msg.collision_objects: + melodic_msg.collision_objects.append( + self._convert_noetic_collision_object_msg_to_melodic(obj) + ) + melodic_msg.octomap = planning_scene_world_msg.octomap + return melodic_msg + + def _convert_melodic_robot_state_msg_to_noetic(self, robot_state_msg): + """ + :param robot_state_msg: Melodic RobotState message + :type robot_state_msg: moveit_msgs.msg.RobotState + :returns: Noetic RobotState message + :rtype: moveit_msgs.msg.RobotState + """ + noetic_msg = RobotState() + noetic_msg.joint_state = robot_state_msg.joint_state + noetic_msg.multi_dof_joint_state = robot_state_msg.multi_dof_joint_state + for obj in robot_state_msg.attached_collision_objects: + noetic_msg.attached_collision_objects.append( + self._convert_melodic_attached_collision_object_msg_to_noetic(obj) + ) + noetic_msg.is_diff = robot_state_msg.is_diff + return noetic_msg + + def _convert_noetic_robot_state_msg_to_melodic(self, robot_state_msg): + """ + :param robot_state_msg: Noetic RobotState message + :type robot_state_msg: moveit_msgs.msg.RobotState + :returns: Melodic RobotState message + :rtype: moveit_msgs.msg.RobotState + """ + melodic_msg = self._melodic_robot_state.RobotState() + melodic_msg.joint_state = robot_state_msg.joint_state + melodic_msg.multi_dof_joint_state = robot_state_msg.multi_dof_joint_state + melodic_msg.is_diff = robot_state_msg.is_diff + for obj in robot_state_msg.attached_collision_objects: + melodic_msg.attached_collision_objects.append( + self._convert_noetic_attached_collision_object_msg_to_melodic(obj) + ) + return melodic_msg + + +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 moveit_msgs version: {}".format(current_version)) + return + rospy.init_node("moveit_noetic_bridge") + bridge_node = MoveitNoeticBridge() + rospy.spin() + + +if __name__ == "__main__": + main() From aae85d38b3ba65cfb73c5e529e947d2f1cfe2d30 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sun, 15 Oct 2023 16:18:33 +0900 Subject: [PATCH 02/10] [jsk_fetch_startup][noetic] Fix client eus code for noetic --- jsk_fetch_robot/fetcheus/fetch-interface.l | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/jsk_fetch_robot/fetcheus/fetch-interface.l b/jsk_fetch_robot/fetcheus/fetch-interface.l index 5dd33840df..d3699d2525 100644 --- a/jsk_fetch_robot/fetcheus/fetch-interface.l +++ b/jsk_fetch_robot/fetcheus/fetch-interface.l @@ -547,7 +547,9 @@ Example: (send self :gripper :position) => 0.00" :super moveit-environment) (defmethod fetch-moveit-environment (:init (&key ((:robot rb) *fetch*) &rest args) - (send-super* :init :robot rb :frame-id "base_link" args)) + (send-super* :init :robot rb :frame-id "base_link" :scene-service "/get_planning_scene_noetic" :planning-service "/plan_kinematic_path_noetic" :state-validity-service "/check_state_validity_noetic" :compute-ik-service "/compute_ik_noetic" args) + ;; FIXME *_noetic should be only when ROS_DISTRO is noetic + ;; TODO /apply_planning_scene (:default-configuration () (list (list :rarm (cons :group-name "arm") From a54c12c27243e87177bf94ca20587f773c20e4e4 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sun, 22 Oct 2023 15:30:15 +0900 Subject: [PATCH 03/10] [jsk_fetch_startup] add topic bridge --- .../scripts/moveit_noetic_bridge.py | 106 +++++++++++++++++- 1 file changed, 105 insertions(+), 1 deletion(-) 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 b3611019fa..33d483ca2e 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,5 +1,6 @@ #!/usr/bin/env python +import actionlib import imp import rospkg import rospy @@ -10,6 +11,8 @@ PlanningScene, PlanningSceneWorld, RobotState, + MoveGroupAction, + MoveGroupResult, ) # importing noetic version from moveit_msgs.srv import ( ApplyPlanningScene, @@ -19,7 +22,6 @@ GetPositionIK, ) # importing noetic version from packaging import version -from rospy.impl.tcpros_pubsub import check_if_still_publisher """ @@ -159,6 +161,22 @@ def __init__(self): "RobotState", "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_RobotState.py", ) + self._melodic_move_group_action = imp.load_source( + "MoveGroupAction", + "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupAction.py", + ) + self._melodic_move_group_goal = imp.load_source( + "MoveGroupGoal", + "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupGoal.py", + ) + self._melodic_move_group_status = imp.load_source( + "MoveGroupStatus", + "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupStatus.py", + ) + self._melodic_move_group_result = imp.load_source( + "MoveGroupResult", + "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupResult.py", + ) # Service bridge self.apply_planning_scene_srv = rospy.Service( @@ -203,6 +221,30 @@ def __init__(self): "/plan_kinematic_path", self._melodic_get_motion_plan.GetMotionPlan ) + # Actionlib + self.move_group_as = actionlib.SimpleActionServer( + "/move_group_noetic", + MoveGroupAction, + execute_cb=self._move_group_action_cb, + auto_start=True, + ) + self.move_group_ac = actionlib.SimpleActionClient( + "/move_group", self._melodic_move_group_action.MoveGroupAction + ) + + # Topic + self.planning_scene_world_cb = rospy.Subscriber( + "/planning_scene_world_noetic", + PlanningSceneWorld, + self._planning_scene_world_cb, + ) + + self.planning_scene_world_pub = rospy.Publisher( + "/planning_scene_world", + self._melodic_planning_scene_world.PlanningSceneWorld, + queue_size=1, + ) + def _apply_planning_scene_srv_cb(self, request): bridged_request = self._melodic_apply_planning_scene.ApplyPlanningSceneRequest() bridged_request.scene = self._convert_noetic_planning_scene_msg_to_melodic( @@ -267,6 +309,24 @@ def _plan_kinematic_path_srv_cb(self, request): ) return response + def _move_group_action_cb(self, goal): + self.move_group_ac.send_goal( + self._convert_noetic_move_group_goal_msg_to_melodic(goal), + feedback_cb=self._move_group_feedback_cb, + ) + self.move_group_ac.wait_for_result() + result = self.move_group_ac.get_result() + self.move_group_as.set_succeeded( + self._convert_melodic_move_group_result_msg_to_noetic(result) + ) + + def _move_group_feedback_cb(self, feedback): + self.move_group_as.publish_feedback(feedback) + + def _planning_scene_world_cb(self, msg): + converted_msg = self._convert_noetic_planning_scene_world_msg_to_melodic(msg) + self.planning_scene_world_pub.publish(converted_msg) + def _convert_melodic_collision_object_msg_to_noetic(self, collision_object_msg): """ :param collision_object_msg: Melodic CollisionObject message @@ -394,6 +454,50 @@ def _convert_melodic_motion_plan_response_msg_to_noetic( noetic_msg.planning_time = motion_plan_response_msg.planning_time noetic_msg.error_code = motion_plan_response_msg.error_code + def _convert_noetic_move_group_goal_msg_to_melodic(self, move_group_goal_msg): + melodic_msg = self._melodic_move_group_goal.MoveGroupGoal() + melodic_msg.request = self._convert_noetic_motion_plan_request_msg_to_melodic( + move_group_goal_msg.request + ) + melodic_msg.planning_options.planning_scene_diff = ( + self._convert_noetic_planning_scene_msg_to_melodic( + move_group_goal_msg.planning_options.planning_scene_diff + ) + ) + melodic_msg.planning_options.plan_only = ( + move_group_goal_msg.planning_options.plan_only + ) + melodic_msg.planning_options.look_around = ( + move_group_goal_msg.planning_options.look_around + ) + melodic_msg.planning_options.look_around_attempts = ( + move_group_goal_msg.planning_options.look_around_attempts + ) + melodic_msg.planning_options.max_safe_execution_cost = ( + move_group_goal_msg.planning_options.max_safe_execution_cost + ) + melodic_msg.planning_options.replan = ( + move_group_goal_msg.planning_options.replan + ) + melodic_msg.planning_options.replan_attempts = ( + move_group_goal_msg.planning_options.replan_attampts + ) + melodic_msg.planning_options.replan_delay = ( + move_group_goal_msg.planning_options.replan_delay + ) + return melodic_msg + + def _convert_melodic_move_group_result_msg_to_noetic(self, move_group_result_msg): + noetic_msg = MoveGroupResult() + noetic_msg.error_code = move_group_result_msg.error_code + noetic_msg.trajectory_start = self._convert_melodic_robot_state_msg_to_noetic( + move_group_result_msg.trajectory_start + ) + noetic_msg.planned_trajectory = move_group_result_msg.planned_trajectory + noetic_msg.executed_trajectory = move_group_result_msg.executed_trajectory + noetic_msg.planning_time = move_group_result_msg.planning_time + return noetic_msg + def _convert_noetic_position_ik_request_msg_to_melodic( self, position_ik_request_msg ): From 2c1e84f7df7c44d8bf953ed296c27c4c035b1377 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sun, 22 Oct 2023 15:55:57 +0900 Subject: [PATCH 04/10] fix bug --- .../scripts/moveit_noetic_bridge.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) 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 33d483ca2e..51bce5c46f 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 @@ -163,19 +163,15 @@ def __init__(self): ) self._melodic_move_group_action = imp.load_source( "MoveGroupAction", - "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupAction.py", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupAction.py", ) self._melodic_move_group_goal = imp.load_source( "MoveGroupGoal", - "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupGoal.py", - ) - self._melodic_move_group_status = imp.load_source( - "MoveGroupStatus", - "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupStatus.py", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupGoal.py", ) self._melodic_move_group_result = imp.load_source( "MoveGroupResult", - "/opt/ros/melodic/lib/python2.7/dist-package/moveit_msgs/msg/_MoveGroupResult.py", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupResult.py", ) # Service bridge @@ -226,8 +222,9 @@ def __init__(self): "/move_group_noetic", MoveGroupAction, execute_cb=self._move_group_action_cb, - auto_start=True, + auto_start=False, ) + self.move_group_as.start() self.move_group_ac = actionlib.SimpleActionClient( "/move_group", self._melodic_move_group_action.MoveGroupAction ) @@ -238,7 +235,6 @@ def __init__(self): PlanningSceneWorld, self._planning_scene_world_cb, ) - self.planning_scene_world_pub = rospy.Publisher( "/planning_scene_world", self._melodic_planning_scene_world.PlanningSceneWorld, From a5b382c9a08a673538c1ae0f602fe9d3c462e856 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sun, 22 Oct 2023 16:52:19 +0900 Subject: [PATCH 05/10] stop bridging action. bridge action topic --- .../scripts/moveit_noetic_bridge.py | 66 ++++++++++++------- 1 file changed, 44 insertions(+), 22 deletions(-) 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 51bce5c46f..d0395801b5 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 @@ -8,6 +8,9 @@ AttachedCollisionObject, CollisionObject, MotionPlanResponse, + MoveGroupActionGoal, + MoveGroupActionResult, + MoveGroupActionFeedback, PlanningScene, PlanningSceneWorld, RobotState, @@ -217,20 +220,38 @@ def __init__(self): "/plan_kinematic_path", self._melodic_get_motion_plan.GetMotionPlan ) - # Actionlib - self.move_group_as = actionlib.SimpleActionServer( - "/move_group_noetic", - MoveGroupAction, - execute_cb=self._move_group_action_cb, - auto_start=False, + # Topic + self.move_group_goal_sub = rospy.Subscriber( + "/move_group_noetic/goal", + MoveGroupActionGoal, + self._move_group_goal_cb, + ) + self.move_group_goal_pub = rospy.Publisher( + "/move_group/goal", + self._melodic_move_group_goal.MoveGroupActionGoal, + queue_size=1, ) - self.move_group_as.start() - self.move_group_ac = actionlib.SimpleActionClient( - "/move_group", self._melodic_move_group_action.MoveGroupAction + self.move_group_result_sub = rospy.Subscriber( + "/move_group/result", + MoveGroupActionResult, + self._move_group_result_cb, ) - - # Topic - self.planning_scene_world_cb = rospy.Subscriber( + self.move_group_result_pub = rospy.Publisher( + "/move_group_noetic/result", + self._melodic_move_group_result.MoveGroupActionResult, + queue_size=1, + ) + self.move_group_feedback_sub = rospy.Subscriber( + "/move_group/feedback", + self._melodic_move_group_action.MoveGroupActionFeedback, + self._move_group_feedback_cb, + ) + self.move_group_feedback_pub = rospy.Publisher( + "/move_group_noetic/feedback", + MoveGroupActionFeedback, + queue_size=1, + ) + self.planning_scene_world_sub = rospy.Subscriber( "/planning_scene_world_noetic", PlanningSceneWorld, self._planning_scene_world_cb, @@ -305,19 +326,20 @@ def _plan_kinematic_path_srv_cb(self, request): ) return response - def _move_group_action_cb(self, goal): - self.move_group_ac.send_goal( - self._convert_noetic_move_group_goal_msg_to_melodic(goal), - feedback_cb=self._move_group_feedback_cb, + def _move_group_goal_cb(self, action_goal): + action_goal.goal = self._convert_noetic_move_group_goal_msg_to_melodic( + action_goal.goal ) - self.move_group_ac.wait_for_result() - result = self.move_group_ac.get_result() - self.move_group_as.set_succeeded( - self._convert_melodic_move_group_result_msg_to_noetic(result) + self.move_group_goal_pub.publish(action_goal) + + def _move_group_result_cb(self, action_result): + action_result.result = self._convert_melodic_move_group_result_msg_to_noetic( + action_result.result ) + self.move_group_result_pub.publish(action_result) - def _move_group_feedback_cb(self, feedback): - self.move_group_as.publish_feedback(feedback) + def _move_group_feedback_cb(self, action_feedback): + self.move_group_feedback_pub.publish(action_feedback) def _planning_scene_world_cb(self, msg): converted_msg = self._convert_noetic_planning_scene_world_msg_to_melodic(msg) From 5be430ed6e09766df85a6d02eb58c52bba95cc3b Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sun, 22 Oct 2023 17:45:13 +0900 Subject: [PATCH 06/10] fix move_group topic name --- .../scripts/moveit_noetic_bridge.py | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) 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 d0395801b5..b607028b17 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 @@ -168,13 +168,17 @@ def __init__(self): "MoveGroupAction", "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupAction.py", ) - self._melodic_move_group_goal = imp.load_source( - "MoveGroupGoal", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupGoal.py", + self._melodic_move_group_action_goal = imp.load_source( + "MoveGroupActionGoal", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupActionGoal.py", ) - self._melodic_move_group_result = imp.load_source( - "MoveGroupResult", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupResult.py", + self._melodic_move_group_action_result = imp.load_source( + "MoveGroupActionResult", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupActionResult.py", + ) + self._melodic_move_group_action_feedback = imp.load_source( + "MoveGroupActionFeedback", + "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupActionFeedback.py", ) # Service bridge @@ -228,7 +232,7 @@ def __init__(self): ) self.move_group_goal_pub = rospy.Publisher( "/move_group/goal", - self._melodic_move_group_goal.MoveGroupActionGoal, + self._melodic_move_group_action_goal.MoveGroupActionGoal, queue_size=1, ) self.move_group_result_sub = rospy.Subscriber( @@ -238,12 +242,12 @@ def __init__(self): ) self.move_group_result_pub = rospy.Publisher( "/move_group_noetic/result", - self._melodic_move_group_result.MoveGroupActionResult, + self._melodic_move_group_action_result.MoveGroupActionResult, queue_size=1, ) self.move_group_feedback_sub = rospy.Subscriber( "/move_group/feedback", - self._melodic_move_group_action.MoveGroupActionFeedback, + self._melodic_move_group_action_feedback.MoveGroupActionFeedback, self._move_group_feedback_cb, ) self.move_group_feedback_pub = rospy.Publisher( From 557cf521e231067b9b16dc207a5ae3bbfe0dd643 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Sun, 22 Oct 2023 18:08:28 +0900 Subject: [PATCH 07/10] fix wrong service name --- .../jsk_fetch_startup/scripts/moveit_noetic_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 b607028b17..7ddaa7556b 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 @@ -271,7 +271,7 @@ def _apply_planning_scene_srv_cb(self, request): bridged_request.scene = self._convert_noetic_planning_scene_msg_to_melodic( request.scene ) - original_response = self.get_planning_scene_proxy.call(bridged_request) + original_response = self.apply_planning_scene_proxy.call(bridged_request) response = self._melodic_apply_planning_scene.ApplyPlanningSceneResponse() response.success = original_response.success return response From 3e77094c0586a60b17ac9bd269cd229563293ddd Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Wed, 1 Nov 2023 16:05:07 +0900 Subject: [PATCH 08/10] not use imp, use sys.path instead --- .../scripts/moveit_noetic_bridge.py | 248 +++++++++--------- 1 file changed, 118 insertions(+), 130 deletions(-) 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 7ddaa7556b..7c15a81851 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,31 +1,21 @@ #!/usr/bin/env python -import actionlib -import imp +import sys import rospkg import rospy -from moveit_msgs.msg import ( - AttachedCollisionObject, - CollisionObject, - MotionPlanResponse, - MoveGroupActionGoal, - MoveGroupActionResult, - MoveGroupActionFeedback, - PlanningScene, - PlanningSceneWorld, - RobotState, - MoveGroupAction, - MoveGroupResult, -) # importing noetic version -from moveit_msgs.srv import ( - ApplyPlanningScene, - GetMotionPlan, - GetPlanningScene, - GetStateValidity, - GetPositionIK, -) # importing noetic version +import moveit_msgs.msg as noetic_moveit_msg +import moveit_msgs.srv as noetic_moveit_srv from packaging import version +for module in sys.modules.keys(): + if module.startswith("moveit_msgs"): + del sys.modules[module] + +sys.path.insert(0, "/opt/ros/melodic/lib/python2.7/dist-packages") +import moveit_msgs.msg as melodic_moveit_msg +import moveit_msgs.srv as melodic_moveit_srv + +sys.path.pop(0) """ On https://github.com/ros-planning/moveit_msgs/compare/0.10.1...0.11.4, the CollisionObject.msg has breaking changes @@ -114,177 +104,175 @@ class MoveitNoeticBridge(object): def __init__(self): # Loading Melodic version services, c.f. https://stackoverflow.com/questions/67631/how-can-i-import-a-module-dynamically-given-the-full-path - self._melodic_apply_planning_scene = imp.load_source( - "ApplyPlanningScene", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_ApplyPlanningScene.py", - ) - self._melodic_get_motion_plan = imp.load_source( - "GetMotionPlan", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetMotionPlan.py", - ) - self._melodic_get_state_validity = imp.load_source( - "GetStateValidity", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetStateValidity.py", - ) - self._melodic_get_position_ik = imp.load_source( - "GetPositionIK", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPositionIK.py", - ) - self._melodic_get_planning_scene = imp.load_source( - "GetPlanningScene", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPlanningScene.py", - ) + # self._melodic_apply_planning_scene = imp.load_source( + # "ApplyPlanningScene", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_ApplyPlanningScene.py", + # ) + # self._melodic_get_motion_plan = imp.load_source( + # "GetMotionPlan", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetMotionPlan.py", + # ) + # self._melodic_get_state_validity = imp.load_source( + # "GetStateValidity", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetStateValidity.py", + # ) + # self._melodic_get_position_ik = imp.load_source( + # "GetPositionIK", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPositionIK.py", + # ) + # self._melodic_get_planning_scene = imp.load_source( + # "GetPlanningScene", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPlanningScene.py", + # ) # Loading Melodic version messages - self._melodic_collision_object = imp.load_source( - "CollisionObject", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_CollisionObject.py", - ) - self._melodic_attached_collision_object = imp.load_source( - "AttachedCollisionObject", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_AttachedCollisionObject.py", - ) - self._melodic_motion_plan_request = imp.load_source( - "MotionPlanRequest", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MotionPlanRequest.py", - ) - self._melodic_position_ik_request = imp.load_source( - "PositionIKRequest", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PositionIKRequest.py", - ) - self._melodic_planning_scene = imp.load_source( - "PlanningScene", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningScene.py", - ) - self._melodic_planning_scene_world = imp.load_source( - "PlanningSceneWorld", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningSceneWorld.py", - ) - self._melodic_robot_state = imp.load_source( - "RobotState", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_RobotState.py", - ) - self._melodic_move_group_action = imp.load_source( - "MoveGroupAction", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupAction.py", - ) - self._melodic_move_group_action_goal = imp.load_source( - "MoveGroupActionGoal", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupActionGoal.py", - ) - self._melodic_move_group_action_result = imp.load_source( - "MoveGroupActionResult", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupActionResult.py", - ) - self._melodic_move_group_action_feedback = imp.load_source( - "MoveGroupActionFeedback", - "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupActionFeedback.py", - ) + # self._melodic_collision_object = imp.load_source( + # "CollisionObject", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_CollisionObject.py", + # ) + # self._melodic_attached_collision_object = imp.load_source( + # "AttachedCollisionObject", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_AttachedCollisionObject.py", + # ) + # self._melodic_motion_plan_request = imp.load_source( + # "MotionPlanRequest", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MotionPlanRequest.py", + # ) + # self._melodic_position_ik_request = imp.load_source( + # "PositionIKRequest", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PositionIKRequest.py", + # ) + # self._melodic_planning_scene = imp.load_source( + # "PlanningScene", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningScene.py", + # ) + # self._melodic_planning_scene_world = imp.load_source( + # "PlanningSceneWorld", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningSceneWorld.py", + # ) + # self._melodic_robot_state = imp.load_source( + # "RobotState", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_RobotState.py", + # ) + # self._melodic_move_group_action = imp.load_source( + # "MoveGroupAction", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupAction.py", + # ) + # self._melodic_move_group_goal = imp.load_source( + # "MoveGroupGoal", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupGoal.py", + # ) + # self._melodic_move_group_result = imp.load_source( + # "MoveGroupResult", + # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupResult.py", + # ) # Service bridge self.apply_planning_scene_srv = rospy.Service( "/apply_planning_scene_noetic", - ApplyPlanningScene, + noetic_moveit_srv.ApplyPlanningScene, self._apply_planning_scene_srv_cb, ) self.check_state_validity_srv = rospy.Service( "/check_state_validity_noetic", - GetStateValidity, + noetic_moveit_srv.GetStateValidity, self._check_state_validity_srv_cb, ) self.compute_ik_srv = rospy.Service( - "/compute_ik_noetic", GetPositionIK, self._compute_ik_srv_cb + "/compute_ik_noetic", + noetic_moveit_srv.GetPositionIK, + self._compute_ik_srv_cb, ) self.get_planning_scene_srv = rospy.Service( "/get_planning_scene_noetic", - GetPlanningScene, + noetic_moveit_srv.GetPlanningScene, self._get_planning_scene_srv_cb, ) self.plan_kinematic_path_srv = rospy.Service( "/plan_kinematic_path_noetic", - GetMotionPlan, + noetic_moveit_srv.GetMotionPlan, self._plan_kinematic_path_srv_cb, ) # NOTE Is this service necessary to be bridged? Not seen in pr2eus_moveit # Service proxy self.apply_planning_scene_proxy = rospy.ServiceProxy( "/apply_planning_scene", - self._melodic_apply_planning_scene.ApplyPlanningScene, + melodic_moveit_srv.ApplyPlanningScene, ) self.check_state_validity_proxy = rospy.ServiceProxy( - "/check_state_validity", self._melodic_get_state_validity.GetStateValidity + "/check_state_validity", melodic_moveit_srv.GetStateValidity ) self.compute_ik_proxy = rospy.ServiceProxy( - "/compute_ik", self._melodic_get_position_ik.GetPositionIK + "/compute_ik", melodic_moveit_srv.GetPositionIK ) self.get_planning_scene_proxy = rospy.ServiceProxy( - "/get_planning_scene", self._melodic_get_planning_scene.GetPlanningScene + "/get_planning_scene", melodic_moveit_srv.GetPlanningScene ) self.plan_kinematic_path_proxy = rospy.ServiceProxy( - "/plan_kinematic_path", self._melodic_get_motion_plan.GetMotionPlan + "/plan_kinematic_path", melodic_moveit_srv.GetMotionPlan ) # Topic self.move_group_goal_sub = rospy.Subscriber( "/move_group_noetic/goal", - MoveGroupActionGoal, + noetic_moveit_msg.MoveGroupActionGoal, self._move_group_goal_cb, ) self.move_group_goal_pub = rospy.Publisher( "/move_group/goal", - self._melodic_move_group_action_goal.MoveGroupActionGoal, + melodic_moveit_msg.MoveGroupActionGoal, queue_size=1, ) self.move_group_result_sub = rospy.Subscriber( "/move_group/result", - MoveGroupActionResult, + noetic_moveit_msg.MoveGroupActionResult, self._move_group_result_cb, ) self.move_group_result_pub = rospy.Publisher( "/move_group_noetic/result", - self._melodic_move_group_action_result.MoveGroupActionResult, + melodic_moveit_msg.MoveGroupActionResult, queue_size=1, ) self.move_group_feedback_sub = rospy.Subscriber( "/move_group/feedback", - self._melodic_move_group_action_feedback.MoveGroupActionFeedback, + melodic_moveit_msg.MoveGroupActionFeedback, self._move_group_feedback_cb, ) self.move_group_feedback_pub = rospy.Publisher( "/move_group_noetic/feedback", - MoveGroupActionFeedback, + noetic_moveit_msg.MoveGroupActionFeedback, queue_size=1, ) self.planning_scene_world_sub = rospy.Subscriber( "/planning_scene_world_noetic", - PlanningSceneWorld, + noetic_moveit_msg.PlanningSceneWorld, self._planning_scene_world_cb, ) self.planning_scene_world_pub = rospy.Publisher( "/planning_scene_world", - self._melodic_planning_scene_world.PlanningSceneWorld, + melodic_moveit_msg.PlanningSceneWorld, queue_size=1, ) def _apply_planning_scene_srv_cb(self, request): - bridged_request = self._melodic_apply_planning_scene.ApplyPlanningSceneRequest() + bridged_request = melodic_moveit_srv.ApplyPlanningSceneRequest() bridged_request.scene = self._convert_noetic_planning_scene_msg_to_melodic( request.scene ) - original_response = self.apply_planning_scene_proxy.call(bridged_request) - response = self._melodic_apply_planning_scene.ApplyPlanningSceneResponse() + original_response = self.get_planning_scene_proxy.call(bridged_request) + response = melodic_moveit_srv.ApplyPlanningSceneResponse() response.success = original_response.success return response def _check_state_validity_srv_cb(self, request): - bridged_request = self._melodic_get_state_validity.GetStateValidityRequest() + bridged_request = melodic_moveit_srv.GetStateValidityRequest() bridged_request.robot_state = self._convert_noetic_robot_state_msg_to_melodic( request.robot_state ) bridged_request.group_name = request.group_name bridged_request.constraints = request.constraints original_response = self.check_state_validity_proxy.call(bridged_request) - response = self._melodic_get_state_validity.GetStateValidityResponse() + response = melodic_moveit_srv.GetStateValidityResponse() response.valid = original_response.valid response.contacts = original_response.contacts response.cost_sources = original_response.cost_sources @@ -292,12 +280,12 @@ def _check_state_validity_srv_cb(self, request): return response def _compute_ik_srv_cb(self, request): - bridged_request = self._melodic_get_position_ik.GetPositionIKRequest() + bridged_request = melodic_moveit_srv.GetPositionIKRequest() bridged_request.ik_request = ( self._convert_noetic_position_ik_request_msg_to_melodic(request.ik_request) ) original_response = self.compute_ik_proxy.call(bridged_request) - response = self._melodic_get_position_ik.GetPositionIKResponse() + response = melodic_moveit_srv.GetPositionIKResponse() response.solution = self._convert_melodic_robot_state_msg_to_noetic( original_response.solution ) @@ -305,24 +293,24 @@ def _compute_ik_srv_cb(self, request): return response def _get_planning_scene_srv_cb(self, request): - bridged_request = self._melodic_get_planning_scene.GetPlanningSceneRequest() + bridged_request = melodic_moveit_srv.GetPlanningSceneRequest() bridged_request.components = request.components original_response = self.get_planning_scene_proxy.call(bridged_request) - response = self._melodic_get_planning_scene.GetPlanningSceneResponse() + response = melodic_moveit_srv.GetPlanningSceneResponse() response.scene = self._convert_melodic_planning_scene_msg_to_noetic( original_response.scene ) return response def _plan_kinematic_path_srv_cb(self, request): - bridged_request = self._melodic_get_motion_plan.GetMotionPlanRequest() + bridged_request = melodic_moveit_srv.GetMotionPlanRequest() bridged_request.motion_plan_request = ( self._convert_noetic_motion_plan_request_msg_to_melodic( request.motion_plan_request ) ) original_response = self.plan_kinematic_path_proxy.call(bridged_request) - response = self._melodic_get_motion_plan.GetMotionPlanResponse() + response = melodic_moveit_srv.GetMotionPlanResponse() response.motion_plan_response = ( self._convert_melodic_motion_plan_response_msg_to_noetic( original_response.motion_plan_response @@ -356,7 +344,7 @@ def _convert_melodic_collision_object_msg_to_noetic(self, collision_object_msg): :returns: Noetic CollisionObject message :rtype: moveit_msgs.msg.CollisionObject """ - noetic_msg = CollisionObject() + noetic_msg = noetic_moveit_msg.CollisionObject() noetic_msg.header = collision_object_msg.header noetic_msg.id = collision_object_msg.id noetic_msg.type = collision_object_msg.type @@ -377,7 +365,7 @@ def _convert_noetic_collision_object_msg_to_melodic(self, collision_object_msg): :returns: Melodic CollisionObject message :rtype: moveit_msgs.msg.CollisionObject """ - melodic_msg = self._melodic_collision_object.CollisionObject() + melodic_msg = melodic_moveit_msg.CollisionObject() melodic_msg.header = collision_object_msg.header melodic_msg.id = collision_object_msg.id melodic_msg.type = collision_object_msg.type @@ -400,7 +388,7 @@ def _convert_melodic_attached_collision_object_msg_to_noetic( :returns: Noetic AttachedCollisionObject message :rtype: moveit_msgs.msg.AttachedCollisionObject """ - noetic_msg = AttachedCollisionObject() + noetic_msg = noetic_moveit_msg.AttachedCollisionObject() noetic_msg.link_name = attached_collision_object_msg.link_name noetic_msg.object = self._convert_melodic_collision_object_msg_to_noetic( attached_collision_object_msg.object @@ -419,7 +407,7 @@ def _convert_noetic_attached_collision_object_msg_to_melodic( :returns: Melodic AttachedCollisionObject message :rtype: moveit_msgs.msg.AttachedCollisionObject """ - melodic_msg = self._melodic_attached_collision_object.AttachedCollisionObject() + melodic_msg = melodic_moveit_msg.AttachedCollisionObject() melodic_msg.link_name = attached_collision_object_msg.link_name melodic_msg.object = self._convert_noetic_collision_object_msg_to_melodic( attached_collision_object_msg.object @@ -438,7 +426,7 @@ def _convert_noetic_motion_plan_request_msg_to_melodic( :returns: Noetic MotionPlanRequest message :rtype: moveit_msgs.msg.MotionPlanRequest """ - melodic_msg = self._melodic_motion_plan_request.MotionPlanRequest() + melodic_msg = melodic_moveit_msg.MotionPlanRequest() melodic_msg.workspace_parameters = motion_plan_request_msg.workspace_parameters melodic_msg.start_state = self._convert_noetic_robot_state_msg_to_melodic( motion_plan_request_msg.start_state @@ -467,7 +455,7 @@ def _convert_noetic_motion_plan_request_msg_to_melodic( def _convert_melodic_motion_plan_response_msg_to_noetic( self, motion_plan_response_msg ): - noetic_msg = MotionPlanResponse() + noetic_msg = noetic_moveit_msg.MotionPlanResponse() noetic_msg.trajectory_start = self._convert_melodic_robot_state_msg_to_noetic( motion_plan_response_msg.trajectory_start ) @@ -477,7 +465,7 @@ def _convert_melodic_motion_plan_response_msg_to_noetic( noetic_msg.error_code = motion_plan_response_msg.error_code def _convert_noetic_move_group_goal_msg_to_melodic(self, move_group_goal_msg): - melodic_msg = self._melodic_move_group_goal.MoveGroupGoal() + melodic_msg = melodic_moveit_msg.MoveGroupGoal() melodic_msg.request = self._convert_noetic_motion_plan_request_msg_to_melodic( move_group_goal_msg.request ) @@ -510,7 +498,7 @@ def _convert_noetic_move_group_goal_msg_to_melodic(self, move_group_goal_msg): return melodic_msg def _convert_melodic_move_group_result_msg_to_noetic(self, move_group_result_msg): - noetic_msg = MoveGroupResult() + noetic_msg = noetic_moveit_msg.MoveGroupResult() noetic_msg.error_code = move_group_result_msg.error_code noetic_msg.trajectory_start = self._convert_melodic_robot_state_msg_to_noetic( move_group_result_msg.trajectory_start @@ -529,7 +517,7 @@ def _convert_noetic_position_ik_request_msg_to_melodic( :returns: Melodic PositionIKRequest message :rtype: moveit_msgs.msg.PositionIKRequest """ - melodic_msg = self._melodic_position_ik_request.PositionIKRequest() + melodic_msg = melodic_moveit_msg.PositionIKRequest() melodic_msg.group_name = position_ik_request_msg.group_name melodic_msg.robot_state = self._convert_noetic_robot_state_msg_to_melodic( position_ik_request_msg.robot_state @@ -551,7 +539,7 @@ def _convert_melodic_planning_scene_msg_to_noetic(self, planning_scene_msg): :returns: Noetic PlanningScene message :rtype: moveit_msgs.msg.PlanningScene """ - noetic_msg = PlanningScene() + noetic_msg = noetic_moveit_msg.PlanningScene() noetic_msg.name = planning_scene_msg.name noetic_msg.robot_state = self._convert_melodic_robot_state_msg_to_noetic( planning_scene_msg.robot_state @@ -577,7 +565,7 @@ def _convert_noetic_planning_scene_msg_to_melodic(self, planning_scene_msg): :returns: Melodic PlanningScene message :rtype: moveit_msgs.msg.PlanningScene """ - melodic_msg = self._melodic_planning_scene.PlanningScene() + melodic_msg = melodic_moveit_msg.PlanningScene() melodic_msg.name = planning_scene_msg.name melodic_msg.robot_state = self._convert_noetic_robot_state_msg_to_melodic( planning_scene_msg.robot_state @@ -605,7 +593,7 @@ def _convert_melodic_planning_scene_world_msg_to_noetic( :returns: Noetic PlanningSceneWorld message :rtype: moveit_msgs.msg.PlanningSceneWorld """ - noetic_msg = PlanningSceneWorld() + noetic_msg = noetic_moveit_msg.PlanningSceneWorld() for obj in planning_scene_world_msg.collision_objects: noetic_msg.collision_objects.append( self._convert_melodic_collision_object_msg_to_noetic(obj) @@ -622,7 +610,7 @@ def _convert_noetic_planning_scene_world_msg_to_melodic( :returns: Melodic PlanningSceneWorld message :rtype: moveit_msgs.msg.PlanningSceneWorld """ - melodic_msg = self._melodic_planning_scene_world.PlanningSceneWorld() + melodic_msg = melodic_moveit_msg.PlanningSceneWorld() for obj in planning_scene_world_msg.collision_objects: melodic_msg.collision_objects.append( self._convert_noetic_collision_object_msg_to_melodic(obj) @@ -637,7 +625,7 @@ def _convert_melodic_robot_state_msg_to_noetic(self, robot_state_msg): :returns: Noetic RobotState message :rtype: moveit_msgs.msg.RobotState """ - noetic_msg = RobotState() + noetic_msg = noetic_moveit_msg.RobotState() noetic_msg.joint_state = robot_state_msg.joint_state noetic_msg.multi_dof_joint_state = robot_state_msg.multi_dof_joint_state for obj in robot_state_msg.attached_collision_objects: @@ -654,7 +642,7 @@ def _convert_noetic_robot_state_msg_to_melodic(self, robot_state_msg): :returns: Melodic RobotState message :rtype: moveit_msgs.msg.RobotState """ - melodic_msg = self._melodic_robot_state.RobotState() + melodic_msg = melodic_moveit_msg.RobotState() melodic_msg.joint_state = robot_state_msg.joint_state melodic_msg.multi_dof_joint_state = robot_state_msg.multi_dof_joint_state melodic_msg.is_diff = robot_state_msg.is_diff From 59d095f767053c3465531169f6f4701254cea450 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Wed, 1 Nov 2023 16:41:14 +0900 Subject: [PATCH 09/10] fix service name --- .../jsk_fetch_startup/scripts/moveit_noetic_bridge.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 7c15a81851..01a37601b4 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 @@ -259,7 +259,7 @@ def _apply_planning_scene_srv_cb(self, request): bridged_request.scene = self._convert_noetic_planning_scene_msg_to_melodic( request.scene ) - original_response = self.get_planning_scene_proxy.call(bridged_request) + original_response = self.apply_planning_scene_proxy.call(bridged_request) response = melodic_moveit_srv.ApplyPlanningSceneResponse() response.success = original_response.success return response From 1f1a0d30bb4069f4101d790ee5389b25340179f4 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Thu, 2 Nov 2023 14:01:15 +0900 Subject: [PATCH 10/10] fix service variable type --- .../scripts/moveit_noetic_bridge.py | 80 ++----------------- 1 file changed, 8 insertions(+), 72 deletions(-) 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 01a37601b4..778f1248d4 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 @@ -3,18 +3,18 @@ import sys import rospkg import rospy +from packaging import version + import moveit_msgs.msg as noetic_moveit_msg import moveit_msgs.srv as noetic_moveit_srv -from packaging import version -for module in sys.modules.keys(): +for module in list(sys.modules): if module.startswith("moveit_msgs"): del sys.modules[module] sys.path.insert(0, "/opt/ros/melodic/lib/python2.7/dist-packages") import moveit_msgs.msg as melodic_moveit_msg import moveit_msgs.srv as melodic_moveit_srv - sys.path.pop(0) """ @@ -103,70 +103,6 @@ class MoveitNoeticBridge(object): """ def __init__(self): - # Loading Melodic version services, c.f. https://stackoverflow.com/questions/67631/how-can-i-import-a-module-dynamically-given-the-full-path - # self._melodic_apply_planning_scene = imp.load_source( - # "ApplyPlanningScene", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_ApplyPlanningScene.py", - # ) - # self._melodic_get_motion_plan = imp.load_source( - # "GetMotionPlan", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetMotionPlan.py", - # ) - # self._melodic_get_state_validity = imp.load_source( - # "GetStateValidity", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetStateValidity.py", - # ) - # self._melodic_get_position_ik = imp.load_source( - # "GetPositionIK", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPositionIK.py", - # ) - # self._melodic_get_planning_scene = imp.load_source( - # "GetPlanningScene", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/srv/_GetPlanningScene.py", - # ) - - # Loading Melodic version messages - # self._melodic_collision_object = imp.load_source( - # "CollisionObject", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_CollisionObject.py", - # ) - # self._melodic_attached_collision_object = imp.load_source( - # "AttachedCollisionObject", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_AttachedCollisionObject.py", - # ) - # self._melodic_motion_plan_request = imp.load_source( - # "MotionPlanRequest", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MotionPlanRequest.py", - # ) - # self._melodic_position_ik_request = imp.load_source( - # "PositionIKRequest", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PositionIKRequest.py", - # ) - # self._melodic_planning_scene = imp.load_source( - # "PlanningScene", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningScene.py", - # ) - # self._melodic_planning_scene_world = imp.load_source( - # "PlanningSceneWorld", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_PlanningSceneWorld.py", - # ) - # self._melodic_robot_state = imp.load_source( - # "RobotState", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_RobotState.py", - # ) - # self._melodic_move_group_action = imp.load_source( - # "MoveGroupAction", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupAction.py", - # ) - # self._melodic_move_group_goal = imp.load_source( - # "MoveGroupGoal", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupGoal.py", - # ) - # self._melodic_move_group_result = imp.load_source( - # "MoveGroupResult", - # "/opt/ros/melodic/lib/python2.7/dist-packages/moveit_msgs/msg/_MoveGroupResult.py", - # ) - # Service bridge self.apply_planning_scene_srv = rospy.Service( "/apply_planning_scene_noetic", @@ -260,7 +196,7 @@ def _apply_planning_scene_srv_cb(self, request): request.scene ) original_response = self.apply_planning_scene_proxy.call(bridged_request) - response = melodic_moveit_srv.ApplyPlanningSceneResponse() + response = noetic_moveit_srv.ApplyPlanningSceneResponse() response.success = original_response.success return response @@ -272,7 +208,7 @@ def _check_state_validity_srv_cb(self, request): bridged_request.group_name = request.group_name bridged_request.constraints = request.constraints original_response = self.check_state_validity_proxy.call(bridged_request) - response = melodic_moveit_srv.GetStateValidityResponse() + response = noetic_moveit_srv.GetStateValidityResponse() response.valid = original_response.valid response.contacts = original_response.contacts response.cost_sources = original_response.cost_sources @@ -285,7 +221,7 @@ def _compute_ik_srv_cb(self, request): self._convert_noetic_position_ik_request_msg_to_melodic(request.ik_request) ) original_response = self.compute_ik_proxy.call(bridged_request) - response = melodic_moveit_srv.GetPositionIKResponse() + response = noetic_moveit_srv.GetPositionIKResponse() response.solution = self._convert_melodic_robot_state_msg_to_noetic( original_response.solution ) @@ -296,7 +232,7 @@ def _get_planning_scene_srv_cb(self, request): bridged_request = melodic_moveit_srv.GetPlanningSceneRequest() bridged_request.components = request.components original_response = self.get_planning_scene_proxy.call(bridged_request) - response = melodic_moveit_srv.GetPlanningSceneResponse() + response = noetic_moveit_srv.GetPlanningSceneResponse() response.scene = self._convert_melodic_planning_scene_msg_to_noetic( original_response.scene ) @@ -310,7 +246,7 @@ def _plan_kinematic_path_srv_cb(self, request): ) ) original_response = self.plan_kinematic_path_proxy.call(bridged_request) - response = melodic_moveit_srv.GetMotionPlanResponse() + response = noetic_moveit_srv.GetMotionPlanResponse() response.motion_plan_response = ( self._convert_melodic_motion_plan_response_msg_to_noetic( original_response.motion_plan_response