From 77739576fc8bc2b2ebf92324482576785c458d80 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 8 Nov 2021 21:14:24 +0400 Subject: [PATCH] :hammer: get stable launch move_group plus gazebo --- rasms_moveit_actions/CMakeLists.txt | 4 +- rasms_moveit_config/config/rasms.rviz | 115 +++++++++++++++++- .../config/rasms_controllers.yml | 7 +- .../config/rasms_moveit_controllers.yml | 4 +- .../launch/rasms_bringup.launch.py | 103 +++++----------- .../launch/rasms_control.launch.py | 50 ++------ .../launch/rasms_moveit_interface.launch.py | 115 ++++++------------ 7 files changed, 197 insertions(+), 201 deletions(-) diff --git a/rasms_moveit_actions/CMakeLists.txt b/rasms_moveit_actions/CMakeLists.txt index 4fb24b0..46857ee 100644 --- a/rasms_moveit_actions/CMakeLists.txt +++ b/rasms_moveit_actions/CMakeLists.txt @@ -58,9 +58,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) add_executable(rasms_moveit src/run_moveit.cpp) -#target_include_directories(rasms_moveit PRIVATE include) +target_include_directories(rasms_moveit PUBLIC include) ament_target_dependencies(rasms_moveit - ${THIS_PACKAGE_INCLUDE_DEPENDS} + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost ) install(TARGETS rasms_moveit diff --git a/rasms_moveit_config/config/rasms.rviz b/rasms_moveit_config/config/rasms.rviz index b1991c0..55bcdb5 100644 --- a/rasms_moveit_config/config/rasms.rviz +++ b/rasms_moveit_config/config/rasms.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /Status1 Splitter Ratio: 0.5 - Tree Height: 814 + Tree Height: 743 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -21,6 +21,8 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui Visualization Manager: Class: "" Displays: @@ -106,6 +108,105 @@ Visualization Manager: Show Robot Collision: false Show Robot Visual: true Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base: + Value: true + link1: + Value: true + link2: + Value: true + link3: + Value: true + link4: + Value: true + link5: + Value: true + link6: + Value: true + world: + Value: true + Marker Scale: 0.5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + base: + link1: + link2: + link3: + link4: + link5: + link6: + {} + Update Interval: 0 + Value: true + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + 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 + base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: /display_planned_path + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -149,7 +250,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.8626145124435425 + Distance: 2.2047624588012695 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -164,10 +265,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.84539794921875 + Pitch: 0.7153979539871216 Target Frame: Value: Orbit (rviz) - Yaw: 0.6953980922698975 + Yaw: 0.7453981041908264 Saved: ~ Window Geometry: Displays: @@ -175,11 +276,15 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001f3000003b9fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b2000002290000000000000000000000010000010f000003b9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003b9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000472000003b900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 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 + RvizVisualToolsGui: + collapsed: false Selection: collapsed: false Tool Properties: collapsed: false + Trajectory - Trajectory Slider: + collapsed: false Views: collapsed: false Width: 1920 diff --git a/rasms_moveit_config/config/rasms_controllers.yml b/rasms_moveit_config/config/rasms_controllers.yml index 7509c11..7e1950b 100644 --- a/rasms_moveit_config/config/rasms_controllers.yml +++ b/rasms_moveit_config/config/rasms_controllers.yml @@ -1,15 +1,14 @@ -# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html controller_manager: ros__parameters: - update_rate: 1000 # Hz + update_rate: 500 # Hz joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - position_trajectory_controller: + rasms_arm_controller: type: joint_trajectory_controller/JointTrajectoryController -position_trajectory_controller: +rasms_arm_controller: ros__parameters: joints: - joint1 diff --git a/rasms_moveit_config/config/rasms_moveit_controllers.yml b/rasms_moveit_config/config/rasms_moveit_controllers.yml index e9397ca..7f360d2 100644 --- a/rasms_moveit_config/config/rasms_moveit_controllers.yml +++ b/rasms_moveit_config/config/rasms_moveit_controllers.yml @@ -1,8 +1,8 @@ controller_names: - - position_trajectory_controller + - rasms_arm_controller # http://control.ros.org/ros2_controllers/joint_trajectory_controller/doc/userdoc.html#ros2-interface-of-the-controller -position_trajectory_controller: +rasms_arm_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory default: true diff --git a/rasms_moveit_config/launch/rasms_bringup.launch.py b/rasms_moveit_config/launch/rasms_bringup.launch.py index ee937c3..e25db11 100644 --- a/rasms_moveit_config/launch/rasms_bringup.launch.py +++ b/rasms_moveit_config/launch/rasms_bringup.launch.py @@ -11,16 +11,32 @@ import xacro import os from ament_index_python import get_package_share_directory +def generate_launch_description(): -def launch_setup(context, *args, **kwargs): + # Launch arguments + launch_args = [] - # Evaluate frequently used variables - model = LaunchConfiguration("model").perform(context) + launch_args.append(DeclareLaunchArgument( + name="robot_name", + default_value="rasms", + description="Set robot name." + ) + ) + + launch_args.append(DeclareLaunchArgument( + name="sim", + default_value="true", + description="Launch robot in simulation or on real setup." + ) + ) + # get xacro file path xacro_file = os.path.join(get_package_share_directory("rasms_description"),"urdf/","rasms_description.urdf.xacro") + # get error if xacro file if missing assert os.path.exists(xacro_file), "The xacro file of rasms_description doesnt exist"+str(xacro_file) + # parse xacro file from file robot_description = xacro.process_file(xacro_file) + # convert file to xml format robot_description_content = robot_description.toxml() - #print(robot_description_content) # Load controls control = IncludeLaunchDescription( @@ -31,10 +47,7 @@ def launch_setup(context, *args, **kwargs): "rasms_control.launch.py" ]) ), launch_arguments=[ - ("robot_description", robot_description_content), - ("controller_configurations_package", LaunchConfiguration("controller_configurations_package")), - ("controller_configurations", LaunchConfiguration("controller_configurations")), - ("sim", LaunchConfiguration("sim")) + ("robot_description", robot_description_content) ] ) @@ -64,76 +77,18 @@ def launch_setup(context, *args, **kwargs): ), launch_arguments=[ ("robot_description", robot_description_content), - ("moveit_controller_configurations_package", LaunchConfiguration("moveit_controller_configurations_package")), - ("moveit_controller_configurations", LaunchConfiguration("moveit_controller_configurations")), - ("model", LaunchConfiguration("model")), ("sim", LaunchConfiguration("sim")) ] ) - return [ - simulation, - control, - move_group - ] + launch_nodes = [] + launch_nodes.append(control) + launch_nodes.append(simulation) + launch_nodes.append(move_group) - -def generate_launch_description(): - - # Launch arguments - launch_args = [] - - launch_args.append(DeclareLaunchArgument( - name="model", - default_value="rasms_description", - description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14." - )) - - launch_args.append(DeclareLaunchArgument( - name="robot_name", - default_value="rasms", - description="Set robot name." - )) - - launch_args.append(DeclareLaunchArgument( - name="sim", - default_value="true", - description="Launch robot in simulation or on real setup." - )) - - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations_package", - default_value="rasms_moveit_config", - description="Package that contains controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations", - default_value="config/rasms_controllers.yml", - description="Relative path to controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations_package", - default_value="rasms_moveit_config", - description="Package that contains MoveIt! controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations", - default_value="config/rasms_moveit_controllers.yml", - description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name. This file lists controllers that are loaded through the controller_configurations file." - ) - ) + return LaunchDescription( - launch_args + [ - OpaqueFunction(function=launch_setup) - ]) + launch_args + + launch_nodes + ) diff --git a/rasms_moveit_config/launch/rasms_control.launch.py b/rasms_moveit_config/launch/rasms_control.launch.py index 070f8c3..6dcbff5 100644 --- a/rasms_moveit_config/launch/rasms_control.launch.py +++ b/rasms_moveit_config/launch/rasms_control.launch.py @@ -16,22 +16,6 @@ def generate_launch_description(): # Launch arguments launch_args = [] - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations_package", - default_value="rasms_moveit_config", - description="Package that contains controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="controller_configurations", - default_value="config/rasms_controllers.yml", - description="Relative path to controller configurations YAML file." - ) - ) - launch_args.append( DeclareLaunchArgument( name="robot_description", @@ -42,27 +26,15 @@ def generate_launch_description(): launch_args.append( DeclareLaunchArgument( name="controller", - default_value="position_trajectory_controller", + default_value="rasms_arm_controller", description="Robot controller." ) ) - launch_args.append( - DeclareLaunchArgument( - name="sim", - default_value="true", - description="Launch robot in simulation or on real setup." - ) - ) - # Configure robot_description robot_description = {"robot_description": LaunchConfiguration("robot_description")} # Load controllers from YAML configuration file - """controller_configurations_path = PathJoinSubstitution([ - FindPackageShare(LaunchConfiguration("controller_configurations_package")), - LaunchConfiguration("controller_configurations") - ])""" ros2_controllers_path = os.path.join( get_package_share_directory("rasms_moveit_config"), "config", @@ -95,21 +67,21 @@ def generate_launch_description(): arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base"], ) - """joint_state_broadcaster = Node( + joint_state_broadcaster = Node( package="controller_manager", executable="spawner.py", arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) - controller = Node( + rasms_arm_controller = Node( package="controller_manager", executable="spawner.py", - arguments=["position_trajectory_controller", "--controller-manager", "/controller_manager"], - )""" + arguments=["rasms_arm_controller", "--controller-manager", "/controller_manager"], + ) - load_controllers = [] + """load_controllers = [] for controller in [ - "position_trajectory_controller", + "rasms_arm_controller", "joint_state_broadcaster", ]: load_controllers += [ @@ -118,14 +90,16 @@ def generate_launch_description(): shell=True, output="screen", ) - ] + ]""" return LaunchDescription( launch_args + - load_controllers + + #load_controllers + [ controller_manager, robot_state_publisher, - static_tf + static_tf, + joint_state_broadcaster, + rasms_arm_controller ] ) diff --git a/rasms_moveit_config/launch/rasms_moveit_interface.launch.py b/rasms_moveit_config/launch/rasms_moveit_interface.launch.py index 168aa6f..a556a98 100644 --- a/rasms_moveit_config/launch/rasms_moveit_interface.launch.py +++ b/rasms_moveit_config/launch/rasms_moveit_interface.launch.py @@ -32,24 +32,45 @@ def load_file(package_name: str, file_path: str) -> str: return None -def launch_setup(context, *args, **kwargs): +def generate_launch_description(): - # Evaluate frequently used variables - model = LaunchConfiguration("model").perform(context) + #============= + #================================================================= Start initialize launch args ========================================================== + #============= - # Configure robot_description + # Launch arguments + launch_args = [] + + launch_args.append( + DeclareLaunchArgument( + name="robot_description", + description="Robot description XML file." + ) + ) + + launch_args.append(DeclareLaunchArgument( + name="sim", + default_value="true", + description="Launch robot in simulation or on real setup." + )) + + #============= + #================================================================= Start launching nodes ========================================================== + #============= + + # Configure robot_description robot_description = {"robot_description": LaunchConfiguration("robot_description")} # Robot semantics SRDF robot_description_semantic = { - "robot_description_semantic": load_file("rasms_moveit_config", "srdf/{}.srdf".format(model)) + "robot_description_semantic": load_file("rasms_moveit_config", "srdf/rasms_description.srdf") } # Kinematics kinematics_yaml = load_yaml("rasms_moveit_config", "config/kinematics.yml") # Update group name - kinematics_yaml["{}_arm".format(model)] = kinematics_yaml["group_name"] + kinematics_yaml["rasms_description_arm"] = kinematics_yaml["group_name"] del kinematics_yaml["group_name"] # Joint limits @@ -79,8 +100,8 @@ def launch_setup(context, *args, **kwargs): # Controllers moveit_simple_controller_yml = load_yaml( - LaunchConfiguration("moveit_controller_configurations_package").perform(context), - LaunchConfiguration("moveit_controller_configurations").perform(context) + "rasms_moveit_config", + "config/rasms_moveit_controllers.yml" ) moveit_controllers = {"moveit_simple_controller_manager": moveit_simple_controller_yml, @@ -110,7 +131,7 @@ def launch_setup(context, *args, **kwargs): # Time configuration use_sim_time = {"use_sime_time": LaunchConfiguration("sim")} - # Prepare move group node + # Simple move_group interface move_group_node = Node( package="moveit_ros_move_group", executable="move_group", @@ -129,8 +150,8 @@ def launch_setup(context, *args, **kwargs): use_sim_time ] ) - - """moveit_cpp_node = Node( + # Node for test moveit_cpp interface + moveit_cpp_node = Node( name="moveit_cpp_actions", package="rasms_moveit_actions", executable="rasms_moveit", @@ -143,20 +164,6 @@ def launch_setup(context, *args, **kwargs): ompl_planning_pipeline_config, moveit_controllers, ], - )""" - - """robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[robot_description] - )""" - - spawn_controller = Node( - package="controller_manager", - executable="spawner.py", - arguments=["joint_state_broadcaster"], - output="screen", ) # RViz @@ -176,57 +183,13 @@ def launch_setup(context, *args, **kwargs): ] ) - return [ - move_group_node, - #moveit_cpp_node, - rviz, - #robot_state_publisher, - spawn_controller - ] + launch_nodes = [] + launch_nodes.append(rviz) + launch_nodes.append(move_group_node) + #launch_nodes.append(moveit_cpp_node) -def generate_launch_description(): - - # Launch arguments - launch_args = [] - - launch_args.append( - DeclareLaunchArgument( - name="robot_description", - description="Robot description XML file." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="model", - default_value="rasms_description", - description="Desired LBR model. Use model:=iiwa7/iiwa14/med7/med14." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations_package", - default_value="rasms_moveit_config", - description="Package that contains MoveIt! controller configurations." - ) - ) - - launch_args.append( - DeclareLaunchArgument( - name="moveit_controller_configurations", - default_value="config/rasms_moveit_controllers.yml", - description="Relative path to MoveIt! controller configurations YAML file. Note that the joints in the controllers must be named according to the robot_name." - ) - ) - - launch_args.append(DeclareLaunchArgument( - name="sim", - default_value="true", - description="Launch robot in simulation or on real setup." - )) return LaunchDescription( - launch_args + [ - OpaqueFunction(function=launch_setup) - ]) \ No newline at end of file + launch_args + + launch_nodes + ) \ No newline at end of file