From 58fd751c6404e8b3436903835cf1ba2c3b09e84e Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Wed, 15 Feb 2023 14:11:12 +0000 Subject: [PATCH] Migrate to new ros_control --- src/ign_world/launch/moveit_launch.py | 210 ++++++++++-------- src/ign_world/world/gaz_new_test.sdf | 4 +- .../config/iisy_gazebo_controllers.yml | 7 + src/iisy_config/urdf/iisy.urdf | 10 +- 4 files changed, 134 insertions(+), 97 deletions(-) create mode 100644 src/iisy_config/config/iisy_gazebo_controllers.yml diff --git a/src/ign_world/launch/moveit_launch.py b/src/ign_world/launch/moveit_launch.py index 5b91ab2..dd5857e 100644 --- a/src/ign_world/launch/moveit_launch.py +++ b/src/ign_world/launch/moveit_launch.py @@ -3,7 +3,7 @@ import subprocess import yaml from launch import LaunchDescription -from launch_ros.actions import Node +from launch_ros.actions import Node,SetParameter from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable, RegisterEventHandler, UnsetEnvironmentVariable from launch_ros.substitutions import FindPackageShare from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution @@ -51,7 +51,7 @@ def load_yaml(package_name, file_path): def generate_launch_description(): robot_name = "iisy" - # pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') + pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros') world_path = os.path.join(FindPackageShare(package="gaz_simulation").find("gaz_simulation"), "world/test2.xml") urdf_path = os.path.join(FindPackageShare(package="iisy_config").find("iisy_config"), "urdf/iisy.urdf") @@ -64,10 +64,21 @@ def generate_launch_description(): kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml') robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml} - ompl_planning_pipeline_config = {'move_group': { - 'planning_plugin': 'ompl_interface/OMPLPlanner', - 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization 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}} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("iisy_config"), + "config", + "iisy_controllers.yaml", + ] + ) + + ompl_planning_pipeline_config = { + 'move_group': { + 'planning_plugin': 'ompl_interface/OMPLPlanner', + 'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization 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('iisy_config', 'config/ompl_planning.yml') ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml) @@ -81,18 +92,25 @@ def generate_launch_description(): } controllers_yaml = load_yaml('iisy_config', 'config/iisy_moveit_controllers.yml') - moveit_controllers = {'moveit_simple_controller_manager': controllers_yaml, - 'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager'} - trajectory_execution = {'moveit_manage_controllers': True, - 'trajectory_execution.allowed_execution_duration_scaling': 1.2, - 'trajectory_execution.allowed_goal_duration_margin': 0.5, - 'trajectory_execution.allowed_start_tolerance': 0.01} + moveit_controllers = { + 'moveit_simple_controller_manager': controllers_yaml, + 'moveit_controller_manager': 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + } - planning_scene_monitor_parameters = {"publish_planning_scene": True, - "publish_geometry_updates": True, - "publish_state_updates": True, - "publish_transforms_updates": True} + trajectory_execution = { + 'moveit_manage_controllers': True, + 'trajectory_execution.allowed_execution_duration_scaling': 1.2, + 'trajectory_execution.allowed_goal_duration_margin': 0.5, + 'trajectory_execution.allowed_start_tolerance': 0.01 + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True + } planning = { "move_group": { @@ -105,13 +123,14 @@ def generate_launch_description(): spawn_robot = Node( package='ros_gz_sim', executable='create', - arguments=['-world', 'empty', '-string', robot_description_config], + arguments=['-world', 'default', '-string', robot_description_config], output='screen' ) robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', + output='screen', parameters=[robot_description] ) @@ -124,7 +143,7 @@ def generate_launch_description(): robot_description_semantic, robot_description_planning, ompl_planning_pipeline_config, - kinematics_yaml, + robot_description_kinematics, planning, trajectory_execution, moveit_controllers, @@ -138,28 +157,48 @@ def generate_launch_description(): # octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml') - return LaunchDescription([ - # SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'), + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') - # IncludeLaunchDescription( + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), + launch_arguments={'gz_args': '-r /home/ros/workspace/src/ign_world/world/gaz_new_test.sdf'}.items(), + ) + + return LaunchDescription([ + SetParameter(name="use_sim_time",value=True), + + #SetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE', value='true'), + #SetEnvironmentVariable(name='DISPLAY', value=':0'), + + #IncludeLaunchDescription( # PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), # launch_arguments={'world': world_path}.items() - # ), + #), - # UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'), + gz_sim, - # Start Gazebo client - # IncludeLaunchDescription( + #Node(package='ros_gz_bridge', + # executable='parameter_bridge', + # name='jointstate_bridge', + # arguments=["/world/default/model/iisy/joint_state"] + #), + + #UnsetEnvironmentVariable(name='LIBGL_ALWAYS_SOFTWARE'), + + #Start Gazebo client + #IncludeLaunchDescription( # PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')) - # ), - - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=spawn_robot, - on_exit=[robot_state_publisher], - ) - ), + #), + + #RegisterEventHandler( + # event_handler=OnProcessExit( + # target_action=spawn_robot, + # on_exit=gzclient, + # ) + #), + Node( package='tf2_ros', @@ -173,7 +212,7 @@ def generate_launch_description(): "--roll", "1.5707963267949", "--pitch", "0", "--yaw", "0", - "--frame-id", "map", + "--frame-id", "world", "--child-frame-id", "lidar_1_link"] ), @@ -189,49 +228,38 @@ def generate_launch_description(): "--roll", "0", "--pitch", "0", "--yaw", "0", - "--frame-id", "map", + "--frame-id", "world", "--child-frame-id", "lidar_2_link"] ), - # Node( - # package='gazebo_ros', - # executable='spawn_entity.py', - # arguments=['-entity', robot_name, - # '-topic', 'robot_description', - # '-x', '-1', - # '-y', '-1', - # '-z', '1', - # '-Y', '0'], # Yaw - # output='screen' - # ), + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=["/clock@rosgraph_msgs/msg/Clock@ignition.msgs.Clock"], + ), - # Node( - # package="gazebo_ros", - # executable="spawn_entity.py", - # arguments=[ - # "-topic", "robot_description", - # "-entity", LaunchConfiguration("iisy") - # ], - # output="screen" - # ), + Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=["/world/default/model/iisy/joint_state@sensor_msgs/msg/JointState@ignition.msgs.Model"], + remappings=[ + ('/world/default/model/iisy/joint_state', '/joint_states'), + ] + ), - # Node( - # package='robot_state_publisher', - # executable='robot_state_publisher', - # name='robot_state_publisher', - # output='both', - # parameters=[robot_description] - # ), + #Node( + # package="controller_manager", + # executable="ros2_control_node", + # parameters=[robot_description, robot_controllers], + # output="both", + #), - - - # , - # Node( - # namespace='turtlesim1', - # executable='gazebo', - # name='gazebo', - # arguments=["test.xml"] - # ) + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='both', + parameters=[robot_description] + ), # Node( # package='controller_manager', @@ -243,26 +271,28 @@ def generate_launch_description(): # ] # ), - Node(package='moveit_ros_move_group', - executable='move_group', + Node( + package='moveit_ros_move_group', + executable='move_group', # prefix='xterm -fs 10 -e gdb --ex run --args', - output='screen', - parameters=[ - robot_description, - robot_description_semantic, - robot_description_planning, - ompl_planning_pipeline_config, - kinematics_yaml, - planning, - trajectory_execution, - moveit_controllers, - planning_scene_monitor_parameters, - # octomap_config, - # octomap_updater_config - ] - ), - + output='screen', + parameters=[ + robot_description, + robot_description_semantic, + robot_description_planning, + ompl_planning_pipeline_config, + robot_description_kinematics, + planning, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + # octomap_config, + # octomap_updater_config + ] + ), + #robot_state_publisher, spawn_robot, - rviz + rviz, + ]) diff --git a/src/ign_world/world/gaz_new_test.sdf b/src/ign_world/world/gaz_new_test.sdf index 91a237b..a1a1816 100644 --- a/src/ign_world/world/gaz_new_test.sdf +++ b/src/ign_world/world/gaz_new_test.sdf @@ -1,5 +1,5 @@ - + 0.001 1.0 @@ -24,7 +24,7 @@ - + /home/ros/walk.dae diff --git a/src/iisy_config/config/iisy_gazebo_controllers.yml b/src/iisy_config/config/iisy_gazebo_controllers.yml new file mode 100644 index 0000000..e359023 --- /dev/null +++ b/src/iisy_config/config/iisy_gazebo_controllers.yml @@ -0,0 +1,7 @@ +# see controllers: http://control.ros.org/ros2_controllers/doc/controllers_index.html +controller_manager: + ros__parameters: + update_rate: 500 # Hz + + joint_state_controller: + type: joint_state_controller/JointStateController # if left empty, all states are published https://github.com/ros-controls/ros2_controllers/blob/b9afbcd74a5263fafa77181187b8ffa8f0696ea8/joint_state_broadcaster/include/joint_state_broadcaster/joint_state_broadcaster.hpp#L41 diff --git a/src/iisy_config/urdf/iisy.urdf b/src/iisy_config/urdf/iisy.urdf index 03e0a16..b853a23 100644 --- a/src/iisy_config/urdf/iisy.urdf +++ b/src/iisy_config/urdf/iisy.urdf @@ -371,12 +371,12 @@ - - iisy + + + + + $(find iisy_config)/config/iisy_controllers.yml - - iisy -