From 75d88027101a3d351a4a4264d7ce2aa803e23e65 Mon Sep 17 00:00:00 2001 From: SteMuc Date: Sun, 28 Apr 2024 17:06:31 +0200 Subject: [PATCH] Added rviz.launch.py --- my_panda_gazebo/launch/simulation.launch.py | 20 +++ .../launch/moveit.launch.py | 4 +- my_panda_moveit_config/launch/rviz.launch.py | 132 ++++++++++++++++++ 3 files changed, 154 insertions(+), 2 deletions(-) create mode 100644 my_panda_moveit_config/launch/rviz.launch.py diff --git a/my_panda_gazebo/launch/simulation.launch.py b/my_panda_gazebo/launch/simulation.launch.py index d44977c..e39c512 100644 --- a/my_panda_gazebo/launch/simulation.launch.py +++ b/my_panda_gazebo/launch/simulation.launch.py @@ -151,6 +151,25 @@ def generate_launch_description(): }.items(), ) + moveit_rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution( + [ + get_package_share_directory("my_panda_moveit_config"), + "launch", + "rviz.launch.py", + ] + ) + ), + launch_arguments={ + "load_gripper": load_gripper, + "robot_ip": robot_ip, + "use_fake_hardware": use_fake_hardware, + "fake_sensor_commands": fake_sensor_commands, + "use_sim": use_sim, + }.items() + ) + return LaunchDescription( [ declare_load_gripper_arg, declare_robot_ip_arg, @@ -160,6 +179,7 @@ def generate_launch_description(): gz_sim, ## Spawn empty world in ignition-gazebo. gz_spawn_entity, ## Spawn Franka Emika Robot on the table. bringup_launch, + moveit_rviz_launch, SetParameter(name="use_sim_time", value=True), ] ) diff --git a/my_panda_moveit_config/launch/moveit.launch.py b/my_panda_moveit_config/launch/moveit.launch.py index 3219365..a7c588c 100644 --- a/my_panda_moveit_config/launch/moveit.launch.py +++ b/my_panda_moveit_config/launch/moveit.launch.py @@ -66,7 +66,7 @@ def generate_launch_description(): robot_description_config = Command( [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper, ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, - ' fake_sensor_commands:=', fake_sensor_commands]) + ' fake_sensor_commands:=', fake_sensor_commands, ' use_sim:=', use_sim]) robot_description = {'robot_description': robot_description_config} @@ -234,7 +234,7 @@ def generate_launch_description(): use_sim_arg = DeclareLaunchArgument( use_sim_parameter_name, default_value='false', - description="Use sim for simulation".format( + description="Use sim for simulation".format( use_sim_parameter_name)) gripper_launch_file = IncludeLaunchDescription( diff --git a/my_panda_moveit_config/launch/rviz.launch.py b/my_panda_moveit_config/launch/rviz.launch.py new file mode 100644 index 0000000..5f7ecfa --- /dev/null +++ b/my_panda_moveit_config/launch/rviz.launch.py @@ -0,0 +1,132 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, Command, FindExecutable + +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare +import yaml + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + +def generate_launch_description(): + + robot_ip_parameter_name = 'robot_ip' + use_fake_hardware_parameter_name = 'use_fake_hardware' + fake_sensor_commands_parameter_name = 'fake_sensor_commands' + load_gripper_parameter_name = 'load_gripper' + use_sim_parameter_name = 'use_sim' + + robot_ip = LaunchConfiguration(robot_ip_parameter_name) + use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name) + fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name) + load_gripper = LaunchConfiguration(load_gripper_parameter_name) + use_sim = LaunchConfiguration(use_sim_parameter_name) + + # planning_context + franka_xacro_file = os.path.join(get_package_share_directory('my_panda_description'), 'robots', + 'panda_arm.urdf.xacro') + robot_description_config = Command( + [FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=', load_gripper, + ' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware, + ' fake_sensor_commands:=', fake_sensor_commands, ' use_sim:=', use_sim]) + + robot_description = {'robot_description': robot_description_config} + + franka_semantic_xacro_file = os.path.join(get_package_share_directory('my_panda_moveit_config'), + 'srdf', + 'panda_arm.srdf.xacro') + robot_description_semantic_config = Command( + [FindExecutable(name='xacro'), ' ', franka_semantic_xacro_file, ' hand:=', load_gripper] + ) + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config + } + + kinematics_yaml = load_yaml( + 'my_panda_moveit_config', 'config/kinematics.yaml' + ) + + # Planning Functionality + ompl_planning_pipeline_config = { + 'move_group': { + 'planning_plugin': 'ompl_interface/OMPLPlanner', + 'request_adapters': 'default_planner_request_adapters/AddTimeOptimalParameterization ' + 'default_planner_request_adapters/ResolveConstraintFrames ' + 'default_planner_request_adapters/FixWorkspaceBounds ' + 'default_planner_request_adapters/FixStartStateBounds ' + 'default_planner_request_adapters/FixStartStateCollision ' + 'default_planner_request_adapters/FixStartStatePathConstraints', + 'start_state_max_bounds_error': 0.1, + } + } + ompl_planning_yaml = load_yaml( + 'my_panda_moveit_config', 'config/ompl_planning.yaml' + ) + ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) + + # RViz + rviz_base = os.path.join(get_package_share_directory('my_panda_moveit_config'), 'rviz') + rviz_full_config = os.path.join(rviz_base, 'moveit.rviz') + + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='log', + arguments=['-d', rviz_full_config], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + ], + ) + + robot_arg = DeclareLaunchArgument( + robot_ip_parameter_name, + description='Hostname or IP address of the robot.') + + use_fake_hardware_arg = DeclareLaunchArgument( + use_fake_hardware_parameter_name, + default_value='false', + description='Use fake hardware') + + fake_sensor_commands_arg = DeclareLaunchArgument( + fake_sensor_commands_parameter_name, + default_value='false', + description="Fake sensor commands. Only valid when '{}' is true".format( + use_fake_hardware_parameter_name)) + + load_gripper_arg = DeclareLaunchArgument( + load_gripper_parameter_name, + default_value='false', + description="Load panda flange gripper".format( + load_gripper_parameter_name)) + + use_sim_arg = DeclareLaunchArgument( + use_sim_parameter_name, + default_value='false', + description="Use sim for simulation".format( + use_sim_parameter_name)) + actions = [ + robot_arg, + use_fake_hardware_arg, + fake_sensor_commands_arg, + load_gripper_arg, + use_sim_arg, + SetParameter(name="use_sim_time", value=use_sim), + rviz_node, + ] + + return LaunchDescription(actions)