-
Notifications
You must be signed in to change notification settings - Fork 97
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[jsk_fetch_startup] wip initial commit of moveit bridge
- Loading branch information
Showing
1 changed file
with
212 additions
and
0 deletions.
There are no files selected for viewing
212 changes: 212 additions & 0 deletions
212
jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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() |