From d19bb7e05331f28d3f30cb86fe3944d3c046b6fa Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Thu, 16 Feb 2023 10:17:54 +0000 Subject: [PATCH] OMPL planning working on occasion, race during startup suspected --- src/ign_world/launch/moveit_launch.py | 94 +++---------------- .../config/iisy_gazebo_controllers.yml | 2 +- 2 files changed, 14 insertions(+), 82 deletions(-) diff --git a/src/ign_world/launch/moveit_launch.py b/src/ign_world/launch/moveit_launch.py index 05f9336..197498f 100644 --- a/src/ign_world/launch/moveit_launch.py +++ b/src/ign_world/launch/moveit_launch.py @@ -50,10 +50,6 @@ def load_yaml(package_name, file_path): def generate_launch_description(): - robot_name = "iisy" - 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") robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf') robot_description = {'robot_description': robot_description_config} @@ -64,13 +60,11 @@ def generate_launch_description(): kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml') robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml} - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare("iisy_config"), - "config", - "iisy_controllers.yaml", - ] - ) + moveit_controllers_yaml = load_yaml("iisy_config", "config/iisy_moveit_controllers.yaml") + moveit_controllers = { + "moveit_simple_controller_manager": moveit_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } ompl_planning_pipeline_config = { 'move_group': { @@ -91,13 +85,6 @@ 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, @@ -127,13 +114,6 @@ def generate_launch_description(): output='screen' ) - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[robot_description] - ) - rviz = Node( package='rviz2', executable='rviz2', @@ -159,50 +139,21 @@ def generate_launch_description(): pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') - 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'), + Node( + package='ros_actor_action_server', + executable='ros_actor_action_server'), - #IncludeLaunchDescription( - # PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')), - # launch_arguments={'world': world_path}.items() - #), - - gz_sim, - - #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=gzclient, - # ) - #), - - + 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(), + ), Node( package='tf2_ros', - namespace='gaz_simulation', executable='static_transform_publisher', name='lidar_1_broadcaster', arguments=[ @@ -218,7 +169,6 @@ def generate_launch_description(): Node( package='tf2_ros', - namespace='gaz_simulation', executable='static_transform_publisher', name='lidar_2_broadcaster', arguments=[ @@ -247,13 +197,6 @@ def generate_launch_description(): ] ), - #Node( - # package="controller_manager", - # executable="ros2_control_node", - # parameters=[robot_description, robot_controllers], - # output="both", - #), - Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -261,21 +204,11 @@ def generate_launch_description(): parameters=[robot_description] ), - # Node( - # package='controller_manager', - # name='gazebo_controller_spawner', - # executable='spawner', - # output='screen', - # parameters=[ - # controllers_yaml, - # ] - # ), - Node( package='moveit_ros_move_group', executable='move_group', # prefix='xterm -fs 10 -e gdb --ex run --args', - output='screen', + output='both', parameters=[ robot_description, robot_description_semantic, @@ -290,7 +223,6 @@ def generate_launch_description(): # octomap_updater_config ] ), - #robot_state_publisher, spawn_robot, rviz, diff --git a/src/iisy_config/config/iisy_gazebo_controllers.yml b/src/iisy_config/config/iisy_gazebo_controllers.yml index e359023..fe24c21 100644 --- a/src/iisy_config/config/iisy_gazebo_controllers.yml +++ b/src/iisy_config/config/iisy_gazebo_controllers.yml @@ -4,4 +4,4 @@ controller_manager: 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 + 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 \ No newline at end of file