update launch files to launch TF publisher
updated parameters for moveit added launch new nodes with parameters
This commit is contained in:
parent
31535e4b10
commit
5f6a6779b4
3 changed files with 94 additions and 40 deletions
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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
|
||||
]
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue