diff --git a/rbs_mill_assist/current.urdf b/rbs_mill_assist/current.urdf deleted file mode 100644 index e69de29..0000000 diff --git a/rbs_mill_assist/launch/moveit.launch.py b/rbs_mill_assist/launch/moveit.launch.py index 51f7fdc..8111d14 100644 --- a/rbs_mill_assist/launch/moveit.launch.py +++ b/rbs_mill_assist/launch/moveit.launch.py @@ -1,3 +1,4 @@ +from launch_param_builder import get_package_share_directory from launch_ros.actions import Node from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction @@ -5,6 +6,7 @@ from launch.substitutions import ( LaunchConfiguration, ) from moveit_configs_utils import MoveItConfigsBuilder, moveit_configs_builder +import os def generate_launch_description(): @@ -95,11 +97,17 @@ def launch_setup(context, *args, **kwargs): .pilz_cartesian_limits("moveit/pilz_cartesian_limits.yaml") .robot_description_kinematics("moveit/kinematics.yaml") .robot_description_semantic("srdf/rbs_arm.srdf", mappings={}) + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) .planning_pipelines( - pipelines=["ompl", "stomp", "chomp"], default_planning_pipeline="ompl" + pipelines=["ompl", "stomp", "chomp", "pilz_industrial_motion_planner"], default_planning_pipeline="ompl" ) .to_moveit_configs() ) + move_group_capabilities = { + "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService" + } planners_ids = [ "RRTstar", "PRM", @@ -121,10 +129,13 @@ def launch_setup(context, *args, **kwargs): namespace=namespace, parameters=[moveit_config.to_dict(), robot_description, use_sim_time], ) + + + places_config_filepath = os.path.join(get_package_share_directory("rbs_mill_assist"), "world/config", "places.yaml") planning_scene_publisher = Node( package="rbs_mill_assist", executable="planning_scene_publisher", - parameters=[moveit_config.to_dict(), {"use_sim_time": True}], + parameters=[moveit_config.to_dict(), {"use_sim_time": True}, {"places_config": places_config_filepath}], ) rviz_node = Node( diff --git a/rbs_mill_assist/launch/simulation.launch.py b/rbs_mill_assist/launch/simulation.launch.py index 603a350..6678104 100644 --- a/rbs_mill_assist/launch/simulation.launch.py +++ b/rbs_mill_assist/launch/simulation.launch.py @@ -1,4 +1,3 @@ -from ntpath import isfile import os import xacro @@ -65,20 +64,28 @@ def launch_setup(context, *args, **kwargs): robot_description_semantic_content = "" if use_moveit.perform(context) == "true": - package_dir = get_package_share_directory(moveit_config_package.perform(context)) - - srdf_file = os.path.join(package_dir, "srdf", moveit_config_file.perform(context)) + package_dir = get_package_share_directory( + moveit_config_package.perform(context) + ) + + srdf_file = os.path.join( + package_dir, "srdf", moveit_config_file.perform(context) + ) if srdf_file.endswith(".xacro"): srdf_config_file = os.path.join(package_dir, "srdf", "xacro_args.yaml") srdf_mappings = load_xacro_args(srdf_config_file, locals()) - robot_description_semantic_content = xacro.process_file(srdf_file, mappings=srdf_mappings) - robot_description_semantic_content = robot_description_semantic_content.toprettyxml(indent=" ") - + robot_description_semantic_content = xacro.process_file( + srdf_file, mappings=srdf_mappings + ) + robot_description_semantic_content = ( + robot_description_semantic_content.toprettyxml(indent=" ") + ) + elif srdf_file.endswith(".srdf"): with open(srdf_file, "r") as file: robot_description_semantic_content = file.read() - + control_space = "joint" control_strategy = "position" interactive = "false" @@ -126,22 +133,38 @@ def launch_setup(context, *args, **kwargs): }.items(), ) - rviz_config_file = os.path.join(description_package_abs_path, "config", "config.rviz") + rviz_config_file = os.path.join( + description_package_abs_path, "config", "config.rviz" + ) rviz_node = Node( package="rviz2", executable="rviz2", arguments=["-d", rviz_config_file], - parameters=[{"use_sim_time": True}] + parameters=[{"use_sim_time": True}], ) - gazebo_world = os.path.join(description_package_abs_path, "world", "default.sdf") + gazebo_world_filepath = os.path.join( + description_package_abs_path, "world", "world.xacro" + ) + gazebo_world = xacro.process_file(gazebo_world_filepath).toxml() + world_sdf_filepath = os.path.join( + description_package_abs_path, "world", "world.sdf" + ) + with open(world_sdf_filepath, "w") as sdf_file: + sdf_file.write(gazebo_world) gazebo = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - [os.path.join(get_package_share_directory('ros_gz_sim'), - 'launch', 'gz_sim.launch.py')]), - launch_arguments=[('gz_args', [' -r ', gazebo_world])], + PythonLaunchDescriptionSource( + [ + os.path.join( + get_package_share_directory("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ) + ] + ), + launch_arguments=[("gz_args", [" -r ", world_sdf_filepath])], ) gazebo_spawn_robot = Node( @@ -160,40 +183,59 @@ def launch_setup(context, *args, **kwargs): gz_bridge = RosGzBridge( bridge_name="ros_gz_bridge", - config_file=os.path.join(description_package_abs_path, "config", "gz_bridge.yaml") + config_file=os.path.join( + description_package_abs_path, "config", "gz_bridge.yaml" + ), ) - grasping_service = Node( - package="rbs_mill_assist", - executable="grasping_service.py" - ) + grasping_service = Node(package="rbs_mill_assist", executable="grasping_service.py") get_named_pose_service = Node( - package="rbs_mill_assist", - executable="get_key_pose_frame.py" + package="rbs_mill_assist", executable="get_key_pose_frame.py" ) - get_workspace = Node( - package="rbs_mill_assist", - executable="get_workspace", - parameters=[ - { - "urdf_path": os.path.join(get_package_share_directory("rbs_mill_assist"), "urdf", "current.urdf"), - "ee_link": ee_link_name, - "use_sim_time": True, - "robot_position": [0.0, 0.0, 0.0] - } - ] + # get_workspace = Node( + # package="rbs_mill_assist", + # executable="get_workspace", + # parameters=[ + # { + # "urdf_path": os.path.join( + # get_package_share_directory("rbs_mill_assist"), + # "urdf", + # "current.urdf", + # ), + # "ee_link": ee_link_name, + # "use_sim_time": True, + # "robot_position": [0.0, 0.0, 0.0], + # } + # ], + # ) + # + # publish_ee_pose = Node( + # package="rbs_mill_assist", + # executable="publish_ee_pose_node", + # parameters=[ + # {"use_sim_time": use_sim_time}, + # {"base_link": base_link_name}, + # {"ee_link": ee_link_name}, + # ], + # ) + + places_config_filepath = os.path.join( + get_package_share_directory("rbs_mill_assist"), "world/config", "places.yaml" ) - publish_ee_pose = Node( + ref_frames_config_filepath = os.path.join( + get_package_share_directory("rbs_mill_assist"), "world/config", "slots.yaml" + ) + places_frame_publisher = Node( package="rbs_mill_assist", - executable="publish_ee_pose_node", + executable="places_frame_publisher", parameters=[ {"use_sim_time": use_sim_time}, - {"base_link": base_link_name}, - {"ee_link": ee_link_name} - ] + {"places_config_filepath": places_config_filepath}, + {"ref_frames_config_filepath": ref_frames_config_filepath} + ], ) nodes_to_start = [ @@ -204,6 +246,7 @@ def launch_setup(context, *args, **kwargs): gz_bridge, grasping_service, get_named_pose_service, + places_frame_publisher, # publish_ee_pose # get_workspace ]