From 802ab0c90d3ee5f1ae7e84ff69cb39917174ff64 Mon Sep 17 00:00:00 2001 From: Yoshiki Obinata Date: Tue, 26 Sep 2023 17:39:15 +0900 Subject: [PATCH] [jsk_fetch_startup] wip initial commit of moveit bridge --- .../scripts/moveit_noetic_bridge.py | 212 ++++++++++++++++++ 1 file changed, 212 insertions(+) create mode 100755 jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py 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..c9baac1a38 --- /dev/null +++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python + +import rospy +from moveit_msgs.msg import CollisionObject, RobotState, AttachedCollisionObject +from rospy.impl.tcpros_pubsub import check_if_still_publisher +from moveit_msgs.srv import GetPlanningScene, GetPlanningSceneRequest, GetPlanningSceneResponse + + +""" +On https://github.com/ros-planning/moveit_msgs/compare/0.10.1...0.11.4, the CollisionObject.msg has breaking changes + +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 topic depends 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): + self._srv_md5sum = { + "moveit_msgs/GetPlanningScene": { + "noetic": "65cf69d3e7a0342e16374024d4eeef65", + "melodic": "2745cf315b4eb5fb00e5befa8714d64d", + }, + "moveit_msgs/GetMotionPlan": { + "noetic": "110dffa73f82b9b8644f525e7fbd9fc4", + "melodic": "657e571ceabcb225c850c02c2249a1e1", + }, + "moveit_msgs/GetStateValidity": { + "noetic": "06ea62db671e4dbf878eaca241db51ad", + "melodic": "0c7c937b6a056e7ae5fded13d8e9b242", + }, + } + + # Service bridge + self.get_planning_scene_srv = rospy.Service( + "/get_planning_scene", rospy.AnyMsg, self._srv_cb + ) + self.plan_kinematic_path_srv = rospy.Service( + "/plan_kinematic_path", rospy.AnyMsg, self._srv_cb + ) + self.check_state_validity_srv = rospy.Service( + "/check_state_validity", rospy.AnyMsg, self._srv_cb + ) + + def _srv_cb(self, request: rospy.AnyMsg): + """ + :param request: service request + :type msg: rospy.AnyMsg + :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 + elif request._type == "moveit_msgs/GetMotionPlan": + pass + elif request._type == "moveit_msgs/GetStateValidity": + pass + + def _is_msg_noetic(self, msg): + """ + :param msg: message + :type msg: rospy.AnyMsg + :returns: True if the message is Noetic message + :rtype: bool + """ + return msg._md5sum == self._srv_md5sum[msg._type]["noetic"] + + def _convert_noetic_robot_state_msg(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 = 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_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.touch_links = obj.touch_links + melodic_collision_object.detach_posture = obj.detach_posture + melodic_collision_object.weight = obj.weight + melodic_msg.attached_collision_objects.append(melodic_collision_object) + return melodic_msg + + def _convert_melodic_collision_object_msg(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 + """ + melodic_msg = 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 + return melodic_msg + + + def _convert_noetic_collision_object_msg(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 = 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 + return melodic_msg + + +if __name__ == "__main__": + rospy.init_node("moveit_noetic_bridge") + bridge_node = MoveitNoeticBridge() + rospy.spin()