Skip to content

Commit

Permalink
obinata wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@mqcmd196
mqcmd196 committed Oct 12, 2023
1 parent 802ab0c commit c6fe263
Show file tree
Hide file tree
Showing 2 changed files with 210 additions and 75 deletions.
91 changes: 47 additions & 44 deletions jsk_fetch_robot/jsk_fetch_startup/package.xml
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
<package>
<package format="3">
<name>jsk_fetch_startup</name>
<version>1.1.0</version>
<description>
Expand All @@ -13,59 +13,62 @@

<build_depend version_gte="0.7.11">fetch_teleop</build_depend>

<run_depend>image_proc</run_depend>
<run_depend>jsk_fetch_accessories</run_depend>
<run_depend>jsk_fetch_diagnosis</run_depend>
<run_depend>jsk_network_tools</run_depend>
<run_depend>jsk_robot_startup</run_depend>
<run_depend version_gte="2.2.12">jsk_tools</run_depend>
<run_depend>jsk_maps</run_depend>
<run_depend>fetcheus</run_depend>
<run_depend>fetch_navigation</run_depend>
<run_depend>fetch_moveit_config</run_depend>
<run_depend>fetch_open_auto_dock</run_depend>
<run_depend>fetch_driver_msgs</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pr2_computer_monitor</run_depend>
<run_depend>robot_pose_publisher</run_depend>
<run_depend>rviz</run_depend>
<run_depend>tf</run_depend>
<run_depend>touchegg</run_depend>
<run_depend>sound_play</run_depend>
<run_depend>switchbot_ros</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>voice_text</run_depend>
<exec_depend>image_proc</exec_depend>
<exec_depend>jsk_fetch_accessories</exec_depend>
<exec_depend>jsk_fetch_diagnosis</exec_depend>
<exec_depend>jsk_network_tools</exec_depend>
<exec_depend>jsk_robot_startup</exec_depend>
<exec_depend version_gte="2.2.12">jsk_tools</exec_depend>
<exec_depend>jsk_maps</exec_depend>
<exec_depend>fetcheus</exec_depend>
<exec_depend>fetch_navigation</exec_depend>
<exec_depend>fetch_moveit_config</exec_depend>
<exec_depend>fetch_open_auto_dock</exec_depend>
<exec_depend>fetch_driver_msgs</exec_depend>
<exec_depend>nodelet</exec_depend>
<exec_depend>pr2_computer_monitor</exec_depend>
<exec_depend>robot_pose_publisher</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>touchegg</exec_depend>
<exec_depend>sound_play</exec_depend>
<exec_depend>switchbot_ros</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>voice_text</exec_depend>

<!-- for fetch_naviation -->
<run_depend>amcl</run_depend>
<run_depend>global_planner</run_depend>
<run_depend>map_server</run_depend>
<run_depend>move_base</run_depend>
<run_depend>move_slow_and_clear</run_depend>
<run_depend>robot_localization</run_depend>
<run_depend>teb_local_planner</run_depend>
<run_depend>pyquaternion</run_depend>
<run_depend>librealsense2</run_depend>
<run_depend>realsense2_camera</run_depend>
<run_depend>realsense2_description</run_depend>
<exec_depend>amcl</exec_depend>
<exec_depend>global_planner</exec_depend>
<exec_depend>map_server</exec_depend>
<exec_depend>move_base</exec_depend>
<exec_depend>move_slow_and_clear</exec_depend>
<exec_depend>robot_localization</exec_depend>
<exec_depend>teb_local_planner</exec_depend>
<exec_depend>pyquaternion</exec_depend>
<exec_depend>librealsense2</exec_depend>
<exec_depend>realsense2_camera</exec_depend>
<exec_depend>realsense2_description</exec_depend>

<!-- for fetch teleop -->
<run_depend>app_manager</run_depend>
<run_depend>fetch_bringup</run_depend>
<run_depend>safe_teleop_base</run_depend>
<run_depend>joy</run_depend>
<run_depend>topic_tools</run_depend>
<run_depend>fetch_teleop</run_depend>
<exec_depend>app_manager</exec_depend>
<exec_depend>fetch_bringup</exec_depend>
<exec_depend>safe_teleop_base</exec_depend>
<exec_depend>joy</exec_depend>
<exec_depend>topic_tools</exec_depend>
<exec_depend>fetch_teleop</exec_depend>

<!-- for diagnostic aggregator -->
<run_depend>diagnostic_aggregator</run_depend>
<exec_depend>diagnostic_aggregator</exec_depend>

<!-- for fetch speech recognition -->
<run_depend>julius_ros</run_depend>
<run_depend version_gte="2.1.13">respeaker_ros</run_depend>
<exec_depend>julius_ros</exec_depend>
<exec_depend version_gte="2.1.13">respeaker_ros</exec_depend>

<!-- for rwt_nav -->
<run_depend>robot_pose_publisher</run_depend>
<exec_depend>robot_pose_publisher</exec_depend>

<exec_depend condition="$ROS_PYTHON_VERSION == 2">python-packaging</exec_depend>
<exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-packaging</exec_depend>

<test_depend>rostest</test_depend>

Expand Down
194 changes: 163 additions & 31 deletions jsk_fetch_robot/jsk_fetch_startup/scripts/moveit_noetic_bridge.py
View file
Original file line number Diff line number Diff line change
@@ -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


"""
Expand Down Expand Up @@ -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
Expand All @@ -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):
"""
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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()

0 comments on commit c6fe263

Please sign in to comment.