From 678070841b7f1aec70caa280c70b9262bb3c0f31 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 23 Oct 2023 13:45:07 -0400 Subject: [PATCH] Fix Panda demo launch for MoveIt 2 tutorials --- panda_moveit_config/launch/demo.launch.py | 45 ++-- panda_moveit_config/launch/moveit.rviz | 234 ++++++++++++++++++- panda_moveit_config/launch/moveit_empty.rviz | 37 ++- 3 files changed, 253 insertions(+), 63 deletions(-) diff --git a/panda_moveit_config/launch/demo.launch.py b/panda_moveit_config/launch/demo.launch.py index 12e74017..cf84d976 100644 --- a/panda_moveit_config/launch/demo.launch.py +++ b/panda_moveit_config/launch/demo.launch.py @@ -1,10 +1,10 @@ import os from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition from launch_ros.actions import Node -from launch.actions import ExecuteProcess +from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory from moveit_configs_utils import MoveItConfigsBuilder @@ -12,8 +12,10 @@ def generate_launch_description(): # Command-line arguments - tutorial_arg = DeclareLaunchArgument( - "rviz_tutorial", default_value="False", description="Tutorial flag" + rviz_config_arg = DeclareLaunchArgument( + "rviz_config", + default_value="moveit.rviz", + description="RViz configuration file", ) db_arg = DeclareLaunchArgument( @@ -23,7 +25,7 @@ def generate_launch_description(): ros2_control_hardware_type = DeclareLaunchArgument( "ros2_control_hardware_type", default_value="mock_components", - description="ROS2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]", + description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]", ) moveit_config = ( @@ -37,6 +39,9 @@ def generate_launch_description(): }, ) .robot_description_semantic(file_path="config/panda.srdf") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") .planning_pipelines( pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"] @@ -54,32 +59,16 @@ def generate_launch_description(): ) # RViz - tutorial_mode = LaunchConfiguration("rviz_tutorial") - rviz_base = os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), "launch" - ) - rviz_full_config = os.path.join(rviz_base, "moveit.rviz") - rviz_empty_config = os.path.join(rviz_base, "moveit_empty.rviz") - rviz_node_tutorial = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_empty_config], - parameters=[ - moveit_config.robot_description, - moveit_config.robot_description_semantic, - moveit_config.planning_pipelines, - moveit_config.robot_description_kinematics, - ], - condition=IfCondition(tutorial_mode), + rviz_base = LaunchConfiguration("rviz_config") + rviz_config = PathJoinSubstitution( + [FindPackageShare("moveit_resources_panda_moveit_config"), "launch", rviz_base] ) rviz_node = Node( package="rviz2", executable="rviz2", name="rviz2", output="log", - arguments=["-d", rviz_full_config], + arguments=["-d", rviz_config], parameters=[ moveit_config.robot_description, moveit_config.robot_description_semantic, @@ -87,7 +76,6 @@ def generate_launch_description(): moveit_config.robot_description_kinematics, moveit_config.joint_limits, ], - condition=UnlessCondition(tutorial_mode), ) # Static TF @@ -159,11 +147,10 @@ def generate_launch_description(): return LaunchDescription( [ - tutorial_arg, + rviz_config_arg, db_arg, ros2_control_hardware_type, rviz_node, - rviz_node_tutorial, static_tf_node, robot_state_publisher, move_group_node, diff --git a/panda_moveit_config/launch/moveit.rviz b/panda_moveit_config/launch/moveit.rviz index 2f6663ee..5421a2d4 100644 --- a/panda_moveit_config/launch/moveit.rviz +++ b/panda_moveit_config/launch/moveit.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 802 + Tree Height: 281 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -19,6 +19,8 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui Visualization Manager: Class: "" Displays: @@ -132,6 +134,7 @@ Visualization Manager: State Display Time: 0.05 s Trail Step Size: 1 Trajectory Topic: /display_planned_path + Use Sim Time: false Value: true - Class: moveit_rviz_plugin/PlanningScene Enabled: true @@ -217,6 +220,206 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: true Value: true + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 3x + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0.25 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: /monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -232,6 +435,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -260,33 +466,39 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.119211196899414 + Distance: 2.718453884124756 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.02386285550892353 - Y: 0.15478567779064178 - Z: 0.039489321410655975 + X: -0.03982115536928177 + Y: 0.10271645337343216 + Z: 0.17354051768779755 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5953981876373291 + Pitch: 0.5503982305526733 Target Frame: Value: Orbit (rviz) - Yaw: 5.958578109741211 + Yaw: 0.6603923439979553 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1025 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000003abfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00ffffff000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e1000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 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 + RvizVisualToolsGui: + collapsed: false Selection: collapsed: false Tool Properties: @@ -295,6 +507,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 + Width: 1850 + X: 70 Y: 27 diff --git a/panda_moveit_config/launch/moveit_empty.rviz b/panda_moveit_config/launch/moveit_empty.rviz index 0680fbfa..f02bcd35 100644 --- a/panda_moveit_config/launch/moveit_empty.rviz +++ b/panda_moveit_config/launch/moveit_empty.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 802 + Tree Height: 787 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -40,18 +40,6 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 100 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rviz_visual_tools - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -67,6 +55,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -95,39 +86,39 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.119211196899414 + Distance: 2.1671347618103027 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.02386285550892353 - Y: 0.15478567779064178 - Z: 0.039489321410655975 + X: -0.03982115536928177 + Y: 0.10271645337343216 + Z: 0.17354051768779755 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.5953981876373291 + Pitch: 0.5503982305526733 Target Frame: Value: Orbit (rviz) - Yaw: 5.958578109741211 + Yaw: 0.6603923439979553 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1025 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000003abfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003ab000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000000000010000010f000003abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000005e1000003ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 1853 - X: 67 + Width: 1850 + X: 70 Y: 27